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 c12aeef..4c48e0a 100644
--- a/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
+++ b/src/main/java/net/onrc/onos/apps/segmentrouting/SegmentRoutingManager.java
@@ -935,11 +935,6 @@
     // Policy routing classes and functions
     // ************************************
 
-    public void removeTunnel(int tunnelId) {
-
-        }
-
-
     class PolicyInfo {
 
         int policyId;
@@ -1003,18 +998,24 @@
      * @param tunnelId  Node IDs for the tunnel
      * @param Ids tunnel ID
      */
-    public boolean createTunnel(int tunnelId, List<String> Ids) {
+    public boolean createTunnel(String tid, List<Dpid> dpids) {
 
+        int tunnelId = Integer.parseInt(tid);
         if (tunnelId < 0) {
             log.debug("Tunnel ID should be posivtive integer.");
             return false;
         }
 
-        if (Ids.isEmpty() || Ids.size() < 2) {
+        if (dpids.isEmpty() || dpids.size() < 2) {
             log.debug("Wrong tunnel information");
             return false;
         }
 
+        List<String> Ids = new ArrayList<String>();
+        for (Dpid dpid: dpids) {
+            Ids.add(getMplsLabel(dpid.toString()));
+        }
+
         HashMap<String, PolicyRouteInfo> stitchingRule = getStitchingRule(Ids);
         stitchInfo.put(Integer.valueOf(tunnelId), stitchingRule);
         if (stitchingRule == null) {
@@ -1262,6 +1263,13 @@
          }
 
 
+    public void removeTunnel(int tunnelId) {
+
+
+
+
+    }
+
     // ************************************
     // Utility functions
     // ************************************
@@ -1542,9 +1550,9 @@
     // Test functions
     // ************************************
 
+    /*
     private void runTest() {
 
-
         if (testMode == POLICY_ADD1) {
             String[] routeArray = {"101", "105", "110"};
             List<String> routeList = new ArrayList<String>();
@@ -1602,6 +1610,71 @@
         }
 
     }
+    */
+
+    private void runTest() {
+
+        if (testMode == POLICY_ADD1) {
+            String[] routeArray = {"101", "105", "110"};
+            List<Dpid> routeList = new ArrayList<Dpid>();
+            for (int i = 0; i < routeArray.length; i++) {
+                Dpid dpid = getSwitchFromNodeId(routeArray[i]).getDpid();
+                routeList.add(dpid);
+            }
+
+            if (createTunnel("1", routeList)) {
+                IPv4Net srcIp = new IPv4Net("10.0.1.1/24");
+                IPv4Net dstIp = new IPv4Net("10.1.2.1/24");
+
+                log.debug("Set the policy 1");
+                this.setPolicyTable(1, null, null, Ethernet.TYPE_IPV4, srcIp,
+                        dstIp, IPv4.PROTOCOL_ICMP, (short)-1, (short)-1, 10000,
+                        1);
+                testMode = POLICY_ADD2;
+                testTask.reschedule(10, TimeUnit.SECONDS);
+            }
+            else {
+                // retry it
+                testTask.reschedule(5, TimeUnit.SECONDS);
+            }
+        }
+        else if (testMode == POLICY_ADD2) {
+            String[] routeArray = {"101", "102", "103", "104", "105", "108",
+                    "110"};
+            List<Dpid> routeList = new ArrayList<Dpid>();
+            for (int i = 0; i < routeArray.length; i++) {
+                Dpid dpid = getSwitchFromNodeId(routeArray[i]).getDpid();
+                routeList.add(dpid);
+            }
+
+            if (createTunnel("2", routeList)) {
+                IPv4Net srcIp = new IPv4Net("10.0.1.1/24");
+                IPv4Net dstIp = new IPv4Net("10.1.2.1/24");
+
+                log.debug("Set the policy 2");
+                this.setPolicyTable(2, null, null, Ethernet.TYPE_IPV4, srcIp,
+                        dstIp, IPv4.PROTOCOL_ICMP, (short)-1, (short)-1, 20000,
+                        2);
+                testMode = POLICY_REMOVE2;
+                testTask.reschedule(10, TimeUnit.SECONDS);
+            }
+            else {
+                log.debug("Retry it");
+                testTask.reschedule(5, TimeUnit.SECONDS);
+            }
+        }
+        else if (testMode == POLICY_REMOVE2){
+            log.debug("Remove the policy 2");
+            this.removePolicy(2);
+            testMode = POLICY_REMOVE1;
+            testTask.reschedule(10, TimeUnit.SECONDS);
+        }
+        else if (testMode == POLICY_REMOVE1){
+            log.debug("Remove the policy 1");
+            this.removePolicy(1);
+        }
+
+    }
 
     private void runTest1() {
 
diff --git a/src/main/java/net/onrc/onos/core/drivermanager/OFSwitchImplCPqD13.java b/src/main/java/net/onrc/onos/core/drivermanager/OFSwitchImplCPqD13.java
index 93632e7..4e11b28 100644
--- a/src/main/java/net/onrc/onos/core/drivermanager/OFSwitchImplCPqD13.java
+++ b/src/main/java/net/onrc/onos/core/drivermanager/OFSwitchImplCPqD13.java
@@ -880,7 +880,6 @@
         return;
     }
 
-
     private void createGroupForMplsLabel(int groupId, String nodeId,
             int nextGroupId, boolean bos) {
         List<BucketInfo> buckets = new ArrayList<BucketInfo>();
@@ -1156,6 +1155,25 @@
         }
     }
 
+    private void deleteGroup(int groupId) {
+
+        List<OFMessage> msglist = new ArrayList<OFMessage>();
+        OFGroup group = OFGroup.of(groupId);
+
+        OFMessage gm = factory.buildGroupDelete()
+                .setGroup(group)
+                .setGroupType(OFGroupType.SELECT)
+                .setXid(getNextTransactionId())
+                .build();
+        msglist.add(gm);
+        try {
+            write(msglist);
+        } catch (IOException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
+    }
+
     private void modifyEcmpGroup(EcmpInfo ecmpInfo) {
         List<OFMessage> msglist = new ArrayList<OFMessage>();
         OFGroup group = OFGroup.of(ecmpInfo.groupId);