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 5befa5b..b724e27 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/ArpHandler.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/ArpHandler.java
@@ -92,20 +92,43 @@
 
     }
 
+    /**
+     * process ARP packets from switches
+     * It add a IP routing rule to the host
+     * If it is an ARP response, then flush out all pending packets to the host
+     *
+     * @param sw
+     * @param inPort
+     * @param payload
+     */
     public void processPacketIn(Switch sw, Port inPort, Ethernet payload){
 
     	log.debug("ArpHandler: Received a ARP packet from sw {} ", sw.getDpid());
 
         ARP arp = (ARP)payload.getPayload();
 
+        byte[] senderMacAddressByte = arp.getSenderHardwareAddress();
+        IPv4Address hostIpAddress = IPv4Address.of(arp.getSenderProtocolAddress());
+        log.debug("ArpHandler: Add IP route to Host {} ", hostIpAddress);
+        srManager.addRouteToHost(sw,hostIpAddress.getInt(), senderMacAddressByte);
+
         if (arp.getOpCode() == ARP.OP_REQUEST) {
         	log.debug("ArpHandler: Received a ARP Requestfrom sw {} ", sw.getDpid());
             handleArpRequest(sw, inPort, payload);
         }
-        byte[] senderMacAddressByte = arp.getSenderHardwareAddress();
-        IPv4Address hostIpAddress = IPv4Address.of(arp.getSenderProtocolAddress());
-    	log.debug("ArpHandler: Add IP route to Host {} ", hostIpAddress);
-        srManager.addRouteToHost(sw,hostIpAddress.getInt(), senderMacAddressByte);
+        else {
+            byte[] destIp = arp.getSenderProtocolAddress();
+            for (IPv4 ipPacket: srManager.getIpPacketFromQueue(destIp)) {
+                if (ipPacket != null && !inSameSubnet(sw, ipPacket)) {
+                    Ethernet eth = new Ethernet();
+                    eth.setDestinationMACAddress(payload.getSourceMACAddress());
+                    eth.setSourceMACAddress(sw.getStringAttribute("routerMac"));
+                    eth.setEtherType(Ethernet.TYPE_IPV4);
+                    eth.setPayload(ipPacket);
+                    sendPacketOut(sw, eth, inPort.getNumber().shortValue());
+                }
+            }
+        }
     }
 
     /**
@@ -172,6 +195,12 @@
     	}
     }
 
+    /**
+     * Check if the ARP request is to known hosts
+     *
+     * @param sw  Switch
+     * @param arpRequest  ARP request to check
+     */
     private Host isArpReqForKnownHost(Switch sw, ARP arpRequest) {
     	Host knownHost = null;
 
@@ -187,6 +216,15 @@
         return knownHost;
 
     }
+
+    /**
+     *
+     * Check if the ARP is for the switch
+     *
+     * @param sw Switch
+     * @param arpRequest ARP request to check
+     * @return true if the ARP is for the switch
+     */
     private boolean isArpReqForSwitch(Switch sw, ARP arpRequest) {
         List<String> subnetGatewayIPs = getSubnetGatewayIps(sw);
         boolean isArpForSwitch = false;
@@ -198,6 +236,7 @@
         }
         return isArpForSwitch;
     }
+
     /**
      * Retrieve Gateway IP address of all subnets defined in net config file
      *
@@ -333,5 +372,55 @@
         flowPusher.add(sw.getDpid(), po);
     }
 
+    /**
+     * Check if the source IP and destination IP are in the same subnet
+     *
+     * @param sw Switch
+     * @param ipv4 IP address to check
+     * @return return true if the IP packet is within the same subnet
+     */
+    private boolean inSameSubnet(Switch sw, IPv4 ipv4) {
+
+        String gwIpSrc = getGwIpForSubnet(ipv4.getSourceAddress());
+        String gwIpDest = getGwIpForSubnet(ipv4.getDestinationAddress());
+
+        if (gwIpSrc.equals(gwIpDest)) {
+            return true;
+        }
+        else
+            return false;
+    }
+
+    /**
+     * Get router IP address for the given IP address
+     *
+     * @param sourceAddress
+     * @return
+     */
+    private String getGwIpForSubnet(int sourceAddress) {
+
+        String gwIp = null;
+        IPv4Address srcIp = IPv4Address.of(sourceAddress);
+
+        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 subnetIpSlash = (String) arry.getJSONObject(i).get("subnetIp");
+                    if (srManager.netMatch(subnetIpSlash, srcIp.toString())) {
+                        gwIp = subnetIpSlash;
+                    }
+                }
+            } catch (JSONException e) {
+                // TODO Auto-generated catch block
+                e.printStackTrace();
+            }
+        }
+
+        return gwIp;
+    }
+
 }
 
diff --git a/src/main/java/net/onrc/onos/apps/segmentrouting/GenericIpHandler.java b/src/main/java/net/onrc/onos/apps/segmentrouting/GenericIpHandler.java
index b0b8cb3..fd27e56 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/GenericIpHandler.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/GenericIpHandler.java
@@ -86,6 +86,7 @@
 
         // Check if the destination is within subnets of the swtich
         if (isWithinSubnets(sw, IPv4Address.of(destinationAddress).toString())) {
+            srManager.addPacket(ipv4);
             srManager.sendArpRequest(sw, destinationAddress, inPort);
         }
     }
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 21b197b..b35f402 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/IcmpHandler.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/IcmpHandler.java
@@ -1,7 +1,18 @@
+
+/*******************************************************************************
+ * Copyright (c) 2014 Open Networking Laboratory.
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Apache License v2.0
+ * which accompanies this distribution, and is available at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ ******************************************************************************/
+
 package net.onrc.onos.apps.segmentrouting;
 
 import java.util.ArrayList;
+import java.util.LinkedList;
 import java.util.List;
+import java.util.Queue;
 
 import net.floodlightcontroller.core.IFloodlightProviderService;
 import net.floodlightcontroller.core.IOFSwitch;
@@ -51,6 +62,8 @@
     private IFlowPusherService flowPusher;
     private boolean controllerPortAllowed = false;
 
+    private Queue<IPv4> icmpQueue = new LinkedList<IPv4>();
+
     private static final int TABLE_VLAN = 0;
     private static final int TABLE_TMAC = 1;
     private static final int TABLE_IPv4_UNICAST = 2;
@@ -78,6 +91,18 @@
         this.srManager = manager;
     }
 
+    /**
+     * handle ICMP packets
+     * If it is for ICMP echo to router IP or any subnet GW IP,
+     * then send ICMP response on behalf of the switch.
+     * If it is for any hosts in subnets of the switches, but if the MAC
+     * address is not known, then send an ARP request to the subent.
+     * If the MAC address is known, then set the routing rule to the switch
+     *
+     * @param sw
+     * @param inPort
+     * @param payload
+     */
     public void processPacketIn(Switch sw, Port inPort, Ethernet payload) {
 
         if (payload.getEtherType() == Ethernet.TYPE_IPV4) {
@@ -88,8 +113,8 @@
 
                 log.debug("ICMPHandler: Received a ICMP packet {} from sw {} ",
                         payload.toString(), sw.getDpid());
-                int destinationAddress = ipv4.getDestinationAddress();
-                String destAddressStr = IPv4Address.of(destinationAddress).toString();
+                IPv4Address destinationAddress =
+                        IPv4Address.of(ipv4.getDestinationAddress());
 
                 // Check if it is ICMP request to the switch
                 String switchIpAddressSlash = sw.getStringAttribute("routerIp");
@@ -99,11 +124,12 @@
                     IPv4Address switchIpAddress = IPv4Address.of(switchIpAddressStr);
                     List<String> gatewayIps = getSubnetGatewayIps(sw);
                     if (((ICMP)ipv4.getPayload()).getIcmpType() == ICMP_TYPE_ECHO &&
-                            (destinationAddress == switchIpAddress.getInt() ||
-                             gatewayIps.contains(destAddressStr))) {
+                            (destinationAddress.getInt() == switchIpAddress.getInt() ||
+                             gatewayIps.contains(destinationAddress.toString()))) {
                         log.debug("ICMPHandler: ICMP packet for sw {} and "
                                 + "sending ICMP response ", sw.getDpid());
                         sendICMPResponse(sw, inPort, payload);
+                        srManager.getIpPacketFromQueue(destinationAddress.getBytes());
                         return;
                     }
                 }
@@ -122,19 +148,21 @@
                                          hostIpAddress);
                         byte[] destinationMacAddress = host.getMacAddress().toBytes();
                         srManager.addRouteToHost(sw,
-                                destinationAddress, destinationMacAddress);
+                                destinationAddress.getInt(), destinationMacAddress);
                         return;
                     }
                 }
                 /* ICMP for an unknown host */
                 log.debug("ICMPHandler: ICMP request for unknown host {}"
                         + " and sending ARP request", destinationAddress);
-                srManager.sendArpRequest(sw, destinationAddress, inPort);
+                srManager.sendArpRequest(sw, destinationAddress.getInt(), inPort);
             }
 
         }
     }
 
+
+
     /**
      * Retrieve Gateway IP address of all subnets defined in net config file
      *
@@ -164,7 +192,6 @@
     }
 
 
-
     /**
      * Send ICMP reply back
      *
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 4f71b41..cb8e5b7 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
@@ -8,8 +8,10 @@
 import java.util.Collection;
 import java.util.HashMap;
 import java.util.Iterator;
+import java.util.LinkedList;
 import java.util.List;
 import java.util.Map;
+import java.util.Queue;
 import java.util.concurrent.ScheduledExecutorService;
 import java.util.concurrent.TimeUnit;
 
@@ -70,6 +72,7 @@
     private ITopologyService topologyService;
     private IPacketService packetService;
     private MutableTopology mutableTopology;
+    private Queue<IPv4> ipPacketQueue;
 
     private List<ArpEntry> arpEntries;
     private ArpHandler arpHandler;
@@ -118,6 +121,7 @@
         threadPool = context.getServiceImpl(IThreadPoolService.class);
         mutableTopology = topologyService.getTopology();
         topologyService.addListener(this, false);
+        ipPacketQueue = new LinkedList<IPv4>();
 
         this.packetService = context.getServiceImpl(IPacketService.class);
         packetService.registerPacketListener(this);
@@ -145,6 +149,7 @@
     	if (payload.getEtherType() == Ethernet.TYPE_ARP)
     		arpHandler.processPacketIn(sw, inPort, payload);
         if (payload.getEtherType() == Ethernet.TYPE_IPV4) {
+            addPacket((IPv4)payload.getPayload());
         	if (((IPv4)payload.getPayload()).getProtocol() == IPv4.PROTOCOL_ICMP)
         		icmpHandler.processPacketIn(sw, inPort, payload);
         	else
@@ -446,10 +451,12 @@
             PushMplsAction pushMplsAction = new PushMplsAction();
             SetMplsIdAction setIdAction = new SetMplsIdAction(Integer.parseInt(mplsLabel));
             CopyTtlOutAction copyTtlOutAction = new CopyTtlOutAction();
+            DecMplsTtlAction decMplsTtlAction = new DecMplsTtlAction(1);
 
             actions.add(pushMplsAction);
-            actions.add(setIdAction);
             actions.add(copyTtlOutAction);
+            actions.add(decMplsTtlAction);
+            actions.add(setIdAction);
         }
         else {
             String fwdToSw = fwdToSws.get(0);
@@ -461,10 +468,12 @@
                 PushMplsAction pushMplsAction = new PushMplsAction();
                 SetMplsIdAction setIdAction = new SetMplsIdAction(Integer.parseInt(mplsLabel));
                 CopyTtlOutAction copyTtlOutAction = new CopyTtlOutAction();
+                DecMplsTtlAction decMplsTtlAction = new DecMplsTtlAction(1);
 
                 actions.add(pushMplsAction);
-                actions.add(setIdAction);
                 actions.add(copyTtlOutAction);
+                actions.add(decMplsTtlAction);
+                actions.add(setIdAction);
             }
         }
 
@@ -484,7 +493,7 @@
                     net.onrc.onos.core.matchaction.MatchActionOperations.Operator.ADD,
                     matchAction);
 
-      IOF13Switch sw13 = (IOF13Switch)floodlightProvider.getMasterSwitch(
+        IOF13Switch sw13 = (IOF13Switch)floodlightProvider.getMasterSwitch(
                 getSwId(sw.getDpid().toString()));
 
         try {
@@ -526,33 +535,29 @@
      * @param fwdSws  next hop switches
      */
     private void setMplsTable(Switch sw, String mplsLabel, List<String> fwdSws) {
-        if (mplsLabel == null) {
-            log.error("mpls label not configured for sw: {}. Not populating"
-                    + " MPLS table entries.", sw.getDpid());
-            return;
-        }
+
         MplsMatch mplsMatch = new MplsMatch(Integer.parseInt(mplsLabel));
 
         List<Action> actions = new ArrayList<Action>();
 
-        if (fwdSws.size() > 1) {
-            DecMplsTtlAction decMplsTtlAction = new DecMplsTtlAction(1);
-            actions.add(decMplsTtlAction);
-        }
         // If the destination is the same as the next hop, then pop MPLS
-        else {
+        if (fwdSws.size() == 1) {
             String fwdMplsId = getMplsLabel(fwdSws.get(0));
             if (fwdMplsId.equals(mplsLabel)) {
                 String fwdSw = fwdSws.get(0);
                 if (mplsLabel.equals(getMplsLabel(fwdSw))) {
                     PopMplsAction popAction = new PopMplsAction(EthType.IPv4);
                     CopyTtlInAction copyTtlInAction = new CopyTtlInAction();
+                    DecNwTtlAction decNwTtlAction = new DecNwTtlAction(1);
 
-                    actions.add(popAction);
                     actions.add(copyTtlInAction);
+                    actions.add(popAction);
+                    actions.add(decNwTtlAction);
                 }
             }
         }
+        DecMplsTtlAction decMplsTtlAction = new DecMplsTtlAction(1);
+        actions.add(decMplsTtlAction);
 
         GroupAction groupAction = new GroupAction();
         for (String fwdSw: fwdSws)
@@ -725,4 +730,38 @@
 
     }
 
+    /**
+     * Add IP packet to a buffer queue
+     *
+     * @param ipv4
+     */
+    public void addPacket(IPv4 ipv4) {
+        synchronized (ipPacketQueue) {
+            ipPacketQueue.add(ipv4);
+        }
+    }
+
+    /**
+     * Retrieve all packets whose destination is the given address.
+     *
+     * @param destIp Destination address of packets to retrieve
+     */
+    public List<IPv4> getIpPacketFromQueue(byte[] destIp) {
+
+        List<IPv4> bufferedPackets = new ArrayList<IPv4>();
+
+        synchronized (ipPacketQueue) {
+            for (IPv4 ip: ipPacketQueue) {
+                int dest = ip.getDestinationAddress();
+                IPv4Address ip1 = IPv4Address.of(dest);
+                IPv4Address ip2 = IPv4Address.of(destIp);
+                if (ip1.equals(ip2)) {
+                    bufferedPackets.add(ipPacketQueue.poll());
+                }
+            }
+        }
+
+        return bufferedPackets;
+    }
+
 }