diff --git a/of/api/src/main/java/org/onlab/onos/of/controller/DefaultPacketContext.java b/of/api/src/main/java/org/onlab/onos/of/controller/DefaultPacketContext.java
index 7ad2fec..8d3d91e 100644
--- a/of/api/src/main/java/org/onlab/onos/of/controller/DefaultPacketContext.java
+++ b/of/api/src/main/java/org/onlab/onos/of/controller/DefaultPacketContext.java
@@ -80,4 +80,9 @@
         return pktin.getInPort().getPortNumber();
     }
 
+    @Override
+    public byte[] unparsed() {
+        return pktin.getData().clone();
+    }
+
 }
diff --git a/of/api/src/main/java/org/onlab/onos/of/controller/PacketContext.java b/of/api/src/main/java/org/onlab/onos/of/controller/PacketContext.java
index c4f785e..536dd7f 100644
--- a/of/api/src/main/java/org/onlab/onos/of/controller/PacketContext.java
+++ b/of/api/src/main/java/org/onlab/onos/of/controller/PacketContext.java
@@ -44,6 +44,12 @@
     public Ethernet parsed();
 
     /**
+     * Provide an unparsed copy of the data.
+     * @return the unparsed form of the payload.
+     */
+    public byte[] unparsed();
+
+    /**
      * Provide the dpid of the switch where the packet in arrived.
      * @return the dpid of the switch.
      */
diff --git a/of/ctl/src/main/java/org/onlab/onos/of/controller/impl/OpenFlowControllerImpl.java b/of/ctl/src/main/java/org/onlab/onos/of/controller/impl/OpenFlowControllerImpl.java
index 2a27ba9..b0a6305 100644
--- a/of/ctl/src/main/java/org/onlab/onos/of/controller/impl/OpenFlowControllerImpl.java
+++ b/of/ctl/src/main/java/org/onlab/onos/of/controller/impl/OpenFlowControllerImpl.java
@@ -41,247 +41,244 @@
             new ConcurrentHashMap<Dpid, OpenFlowSwitch>();
 
     protected OpenFlowSwitchAgent agent = new OpenFlowSwitchAgent();
-    protected Set<OpenFlowSwitchListener> ofEventListener =
-            new HashSet<>();
+    protected Set<OpenFlowSwitchListener> ofEventListener = new HashSet<>();
 
-            protected List<PacketListener> ofPacketListener =
-                    new ArrayList<>();
+    protected List<PacketListener> ofPacketListener = new ArrayList<>();
 
-                    private final Controller ctrl = new Controller();
+    private final Controller ctrl = new Controller();
 
-                    @Activate
-                    public void activate() {
-                        ctrl.start(agent);
+    @Activate
+    public void activate() {
+        ctrl.start(agent);
+    }
+
+    @Deactivate
+    public void deactivate() {
+        ctrl.stop();
+    }
+
+    @Override
+    public Iterable<OpenFlowSwitch> getSwitches() {
+        return connectedSwitches.values();
+    }
+
+    @Override
+    public Iterable<OpenFlowSwitch> getMasterSwitches() {
+        return activeMasterSwitches.values();
+    }
+
+    @Override
+    public Iterable<OpenFlowSwitch> getEqualSwitches() {
+        return activeEqualSwitches.values();
+    }
+
+    @Override
+    public OpenFlowSwitch getSwitch(Dpid dpid) {
+        return connectedSwitches.get(dpid);
+    }
+
+    @Override
+    public OpenFlowSwitch getMasterSwitch(Dpid dpid) {
+        return activeMasterSwitches.get(dpid);
+    }
+
+    @Override
+    public OpenFlowSwitch getEqualSwitch(Dpid dpid) {
+        return activeEqualSwitches.get(dpid);
+    }
+
+    @Override
+    public void addListener(OpenFlowSwitchListener listener) {
+        if (!ofEventListener.contains(listener)) {
+            this.ofEventListener.add(listener);
+        }
+    }
+
+    @Override
+    public void removeListener(OpenFlowSwitchListener listener) {
+        this.ofEventListener.remove(listener);
+    }
+
+    @Override
+    public void addPacketListener(int priority, PacketListener listener) {
+        ofPacketListener.add(priority, listener);
+    }
+
+    @Override
+    public void removePacketListener(PacketListener listener) {
+        ofPacketListener.remove(listener);
+    }
+
+    @Override
+    public void write(Dpid dpid, OFMessage msg) {
+        this.getSwitch(dpid).sendMsg(msg);
+    }
+
+    @Override
+    public void processPacket(Dpid dpid, OFMessage msg) {
+        switch (msg.getType()) {
+        case PORT_STATUS:
+            for (OpenFlowSwitchListener l : ofEventListener) {
+                l.portChanged(dpid, (OFPortStatus) msg);
+            }
+            break;
+        case PACKET_IN:
+            for (PacketListener p : ofPacketListener) {
+                p.handlePacket(DefaultPacketContext
+                        .packetContextFromPacketIn(this.getSwitch(dpid),
+                                (OFPacketIn) msg));
+            }
+            break;
+        default:
+            log.warn("Handling message type {} not yet implemented {}",
+                    msg.getType(), msg);
+        }
+    }
+
+    @Override
+    public void setRole(Dpid dpid, RoleState role) {
+        getSwitch(dpid).setRole(role);
+    }
+
+    /**
+     * Implementation of an OpenFlow Agent which is responsible for
+     * keeping track of connected switches and the state in which
+     * they are.
+     */
+    public class OpenFlowSwitchAgent implements OpenFlowAgent {
+
+        private final Logger log = LoggerFactory.getLogger(OpenFlowSwitchAgent.class);
+        private final Lock switchLock = new ReentrantLock();
+
+        @Override
+        public boolean addConnectedSwitch(Dpid dpid, OpenFlowSwitch sw) {
+            if (connectedSwitches.get(dpid) != null) {
+                log.error("Trying to add connectedSwitch but found a previous "
+                        + "value for dpid: {}", dpid);
+                return false;
+            } else {
+                log.error("Added switch {}", dpid);
+                connectedSwitches.put(dpid, sw);
+                for (OpenFlowSwitchListener l : ofEventListener) {
+                    l.switchAdded(dpid);
+                }
+                return true;
+            }
+        }
+
+        @Override
+        public boolean validActivation(Dpid dpid) {
+            if (connectedSwitches.get(dpid) == null) {
+                log.error("Trying to activate switch but is not in "
+                        + "connected switches: dpid {}. Aborting ..",
+                        dpid);
+                return false;
+            }
+            if (activeMasterSwitches.get(dpid) != null ||
+                    activeEqualSwitches.get(dpid) != null) {
+                log.error("Trying to activate switch but it is already "
+                        + "activated: dpid {}. Found in activeMaster: {} "
+                        + "Found in activeEqual: {}. Aborting ..", new Object[]{
+                                dpid,
+                                (activeMasterSwitches.get(dpid) == null) ? 'N' : 'Y',
+                                        (activeEqualSwitches.get(dpid) == null) ? 'N' : 'Y'});
+                return false;
+            }
+            return true;
+        }
+
+
+        @Override
+        public boolean addActivatedMasterSwitch(Dpid dpid, OpenFlowSwitch sw) {
+            switchLock.lock();
+            try {
+                if (!validActivation(dpid)) {
+                    return false;
+                }
+                activeMasterSwitches.put(dpid, sw);
+                return true;
+            } finally {
+                switchLock.unlock();
+            }
+        }
+
+        @Override
+        public boolean addActivatedEqualSwitch(Dpid dpid, OpenFlowSwitch sw) {
+            switchLock.lock();
+            try {
+                if (!validActivation(dpid)) {
+                    return false;
+                }
+                activeEqualSwitches.put(dpid, sw);
+                log.info("Added Activated EQUAL Switch {}", dpid);
+                return true;
+            } finally {
+                switchLock.unlock();
+            }
+        }
+
+        @Override
+        public void transitionToMasterSwitch(Dpid dpid) {
+            switchLock.lock();
+            try {
+                if (activeMasterSwitches.containsKey(dpid)) {
+                    return;
+                }
+                OpenFlowSwitch sw = activeEqualSwitches.remove(dpid);
+                if (sw == null) {
+                    sw = getSwitch(dpid);
+                    if (sw == null) {
+                        log.error("Transition to master called on sw {}, but switch "
+                                + "was not found in controller-cache", dpid);
+                        return;
                     }
-
-                    @Deactivate
-                    public void deactivate() {
-                        ctrl.stop();
-                    }
-
-                    @Override
-                    public Iterable<OpenFlowSwitch> getSwitches() {
-                        return connectedSwitches.values();
-                    }
-
-                    @Override
-                    public Iterable<OpenFlowSwitch> getMasterSwitches() {
-                        return activeMasterSwitches.values();
-                    }
-
-                    @Override
-                    public Iterable<OpenFlowSwitch> getEqualSwitches() {
-                        return activeEqualSwitches.values();
-                    }
-
-                    @Override
-                    public OpenFlowSwitch getSwitch(Dpid dpid) {
-                        return connectedSwitches.get(dpid);
-                    }
-
-                    @Override
-                    public OpenFlowSwitch getMasterSwitch(Dpid dpid) {
-                        return activeMasterSwitches.get(dpid);
-                    }
-
-                    @Override
-                    public OpenFlowSwitch getEqualSwitch(Dpid dpid) {
-                        return activeEqualSwitches.get(dpid);
-                    }
-
-                    @Override
-                    public void addListener(OpenFlowSwitchListener listener) {
-                        if (!ofEventListener.contains(listener)) {
-                            this.ofEventListener.add(listener);
-                        }
-                    }
-
-                    @Override
-                    public void removeListener(OpenFlowSwitchListener listener) {
-                        this.ofEventListener.remove(listener);
-                    }
-
-                    @Override
-                    public void addPacketListener(int priority, PacketListener listener) {
-                        ofPacketListener.add(priority, listener);
-                    }
-
-                    @Override
-                    public void removePacketListener(PacketListener listener) {
-                        ofPacketListener.remove(listener);
-                    }
-
-                    @Override
-                    public void write(Dpid dpid, OFMessage msg) {
-                        this.getSwitch(dpid).sendMsg(msg);
-                    }
-
-                    @Override
-                    public void processPacket(Dpid dpid, OFMessage msg) {
-                        switch (msg.getType()) {
-                        case PORT_STATUS:
-                            for (OpenFlowSwitchListener l : ofEventListener) {
-                                l.portChanged(dpid, (OFPortStatus) msg);
-                            }
-                            break;
-                        case PACKET_IN:
-                            for (PacketListener p : ofPacketListener) {
-                                //TODO fix me!
-                                p.handlePacket(DefaultPacketContext
-                                        .packetContextFromPacketIn(this.getSwitch(dpid),
-                                                (OFPacketIn) msg));
-                            }
-                            break;
-                        default:
-                            log.warn("Handling message type {} not yet implemented {}",
-                                    msg.getType(), msg);
-                        }
-                    }
-
-                    @Override
-                    public void setRole(Dpid dpid, RoleState role) {
-                        getSwitch(dpid).setRole(role);
-                    }
-
-                    /**
-                     * Implementation of an OpenFlow Agent which is responsible for
-                     * keeping track of connected switches and the state in which
-                     * they are.
-                     */
-                    public class OpenFlowSwitchAgent implements OpenFlowAgent {
-
-                        private final Logger log = LoggerFactory.getLogger(OpenFlowSwitchAgent.class);
-                        private final Lock switchLock = new ReentrantLock();
-
-                        @Override
-                        public boolean addConnectedSwitch(Dpid dpid, OpenFlowSwitch sw) {
-                            if (connectedSwitches.get(dpid) != null) {
-                                log.error("Trying to add connectedSwitch but found a previous "
-                                        + "value for dpid: {}", dpid);
-                                return false;
-                            } else {
-                                log.error("Added switch {}", dpid);
-                                connectedSwitches.put(dpid, sw);
-                                for (OpenFlowSwitchListener l : ofEventListener) {
-                                    l.switchAdded(dpid);
-                                }
-                                return true;
-                            }
-                        }
-
-                        @Override
-                        public boolean validActivation(Dpid dpid) {
-                            if (connectedSwitches.get(dpid) == null) {
-                                log.error("Trying to activate switch but is not in "
-                                        + "connected switches: dpid {}. Aborting ..",
-                                        dpid);
-                                return false;
-                            }
-                            if (activeMasterSwitches.get(dpid) != null ||
-                                    activeEqualSwitches.get(dpid) != null) {
-                                log.error("Trying to activate switch but it is already "
-                                        + "activated: dpid {}. Found in activeMaster: {} "
-                                        + "Found in activeEqual: {}. Aborting ..", new Object[]{
-                                                dpid,
-                                                (activeMasterSwitches.get(dpid) == null) ? 'N' : 'Y',
-                                                        (activeEqualSwitches.get(dpid) == null) ? 'N' : 'Y'});
-                                return false;
-                            }
-                            return true;
-                        }
+                }
+                log.info("Transitioned switch {} to MASTER", dpid);
+                activeMasterSwitches.put(dpid, sw);
+            } finally {
+                switchLock.unlock();
+            }
+        }
 
 
-                        @Override
-                        public boolean addActivatedMasterSwitch(Dpid dpid, OpenFlowSwitch sw) {
-                            switchLock.lock();
-                            try {
-                                if (!validActivation(dpid)) {
-                                    return false;
-                                }
-                                activeMasterSwitches.put(dpid, sw);
-                                return true;
-                            } finally {
-                                switchLock.unlock();
-                            }
-                        }
+        @Override
+        public void transitionToEqualSwitch(Dpid dpid) {
+            switchLock.lock();
+            try {
+                if (activeEqualSwitches.containsKey(dpid)) {
+                    return;
+                }
+                OpenFlowSwitch sw = activeMasterSwitches.remove(dpid);
+                if (sw == null) {
+                    log.error("Transition to equal called on sw {}, but switch "
+                            + "was not found in controller-cache", dpid);
+                    return;
+                }
+                log.info("Transitioned switch {} to EQUAL", dpid);
+                activeEqualSwitches.put(dpid, sw);
+            } finally {
+                switchLock.unlock();
+            }
 
-                        @Override
-                        public boolean addActivatedEqualSwitch(Dpid dpid, OpenFlowSwitch sw) {
-                            switchLock.lock();
-                            try {
-                                if (!validActivation(dpid)) {
-                                    return false;
-                                }
-                                activeEqualSwitches.put(dpid, sw);
-                                log.info("Added Activated EQUAL Switch {}", dpid);
-                                return true;
-                            } finally {
-                                switchLock.unlock();
-                            }
-                        }
+        }
 
-                        @Override
-                        public void transitionToMasterSwitch(Dpid dpid) {
-                            switchLock.lock();
-                            try {
-                                if (activeMasterSwitches.containsKey(dpid)) {
-                                    return;
-                                }
-                                OpenFlowSwitch sw = activeEqualSwitches.remove(dpid);
-                                if (sw == null) {
-                                    sw = getSwitch(dpid);
-                                    if (sw == null) {
-                                        log.error("Transition to master called on sw {}, but switch "
-                                                + "was not found in controller-cache", dpid);
-                                        return;
-                                    }
-                                }
-                                log.info("Transitioned switch {} to MASTER", dpid);
-                                activeMasterSwitches.put(dpid, sw);
-                            } finally {
-                                switchLock.unlock();
-                            }
-                        }
+        @Override
+        public void removeConnectedSwitch(Dpid dpid) {
+            connectedSwitches.remove(dpid);
+            OpenFlowSwitch sw = activeMasterSwitches.remove(dpid);
+            if (sw == null) {
+                sw = activeEqualSwitches.remove(dpid);
+            }
+            for (OpenFlowSwitchListener l : ofEventListener) {
+                l.switchRemoved(dpid);
+            }
+        }
 
-
-                        @Override
-                        public void transitionToEqualSwitch(Dpid dpid) {
-                            switchLock.lock();
-                            try {
-                                if (activeEqualSwitches.containsKey(dpid)) {
-                                    return;
-                                }
-                                OpenFlowSwitch sw = activeMasterSwitches.remove(dpid);
-                                if (sw == null) {
-                                    log.error("Transition to equal called on sw {}, but switch "
-                                            + "was not found in controller-cache", dpid);
-                                    return;
-                                }
-                                log.info("Transitioned switch {} to EQUAL", dpid);
-                                activeEqualSwitches.put(dpid, sw);
-                            } finally {
-                                switchLock.unlock();
-                            }
-
-                        }
-
-                        @Override
-                        public void removeConnectedSwitch(Dpid dpid) {
-                            connectedSwitches.remove(dpid);
-                            OpenFlowSwitch sw = activeMasterSwitches.remove(dpid);
-                            if (sw == null) {
-                                sw = activeEqualSwitches.remove(dpid);
-                            }
-                            for (OpenFlowSwitchListener l : ofEventListener) {
-                                l.switchRemoved(dpid);
-                            }
-                        }
-
-                        @Override
-                        public void processMessage(Dpid dpid, OFMessage m) {
-                            processPacket(dpid, m);
-                        }
-                    }
+        @Override
+        public void processMessage(Dpid dpid, OFMessage m) {
+            processPacket(dpid, m);
+        }
+    }
 
 
 }
diff --git a/providers/of/link/src/main/java/org/onlab/onos/provider/of/link/impl/LinkDiscovery.java b/providers/of/link/src/main/java/org/onlab/onos/provider/of/link/impl/LinkDiscovery.java
index 9396f91..f250660 100644
--- a/providers/of/link/src/main/java/org/onlab/onos/provider/of/link/impl/LinkDiscovery.java
+++ b/providers/of/link/src/main/java/org/onlab/onos/provider/of/link/impl/LinkDiscovery.java
@@ -21,9 +21,9 @@
 import java.util.HashMap;
 import java.util.HashSet;
 import java.util.Iterator;
-import java.util.List;
 import java.util.Map;
 import java.util.Set;
+import java.util.concurrent.ConcurrentHashMap;
 import java.util.concurrent.TimeUnit;
 import java.util.concurrent.atomic.AtomicInteger;
 
@@ -85,6 +85,7 @@
     private final boolean useBDDP;
     private final OpenFlowController ctrl;
     private final LinkProviderService linkProvider;
+    private final Map<Integer, OFPortDesc> ports;
 
     /**
      * Instantiates discovery manager for the given physical switch. Creates a
@@ -103,6 +104,7 @@
         this.linkProvider = providerService;
         this.slowPorts = Collections.synchronizedSet(new HashSet<Integer>());
         this.fastPorts = Collections.synchronizedSet(new HashSet<Integer>());
+        this.ports = new ConcurrentHashMap<>();
         this.portProbeCount = new HashMap<Integer, AtomicInteger>();
         this.lldpPacket = new ONLabLddp();
         this.lldpPacket.setSwitch(this.sw.getId());
@@ -140,7 +142,7 @@
      */
     public void addPort(final OFPortDesc port) {
         // Ignore ports that are not on this switch, or already booted. */
-
+        this.ports.put(port.getPortNo().getPortNumber(), port);
         synchronized (this) {
             this.log.debug("sending init probe to port {}",
                     port.getPortNo().getPortNumber());
@@ -274,12 +276,10 @@
      * Handles an incoming LLDP packet. Creates link in topology and sends ACK
      * to port where LLDP originated.
      */
-    @SuppressWarnings("rawtypes")
-    public void handleLLDP(final Ethernet eth, Integer inPort) {
+    public void handleLLDP(final byte[] pkt, Integer inPort) {
 
-        final byte[] pkt = eth.serialize();
-
-        if (ONLabLddp.isOVXLLDP(pkt)) {
+        short ethType = ONLabLddp.isOVXLLDP(pkt);
+        if (ethType == Ethernet.TYPE_LLDP || ethType == Ethernet.TYPE_BSN) {
             final Integer dstPort = inPort;
             final DPIDandPort dp = ONLabLddp.parseLLDP(pkt);
             final OpenFlowSwitch srcSwitch = ctrl.getSwitch(new Dpid(dp.getDpid()));
@@ -296,7 +296,7 @@
                     DeviceId.deviceId("of:" + Long.toHexString(sw.getId())),
                     PortNumber.portNumber(dstPort));
             LinkDescription ld;
-            if (eth.getEtherType() == Ethernet.TYPE_BSN) {
+            if (ethType == Ethernet.TYPE_BSN) {
                 ld = new DefaultLinkDescription(src, dst, Type.INDIRECT);
             } else {
                 ld = new DefaultLinkDescription(src, dst, Type.DIRECT);
@@ -307,13 +307,8 @@
         }
     }
 
-    private OFPortDesc findPort(List<OFPortDesc> ports, Integer inPort) {
-        for (OFPortDesc p : ports) {
-            if (p.getPortNo().getPortNumber() == inPort) {
-                return p;
-            }
-        }
-        return null;
+    private OFPortDesc findPort(Integer inPort) {
+        return ports.get(inPort);
     }
 
     /**
@@ -333,7 +328,7 @@
                 final Integer portNumber = fastIterator.next();
                 final int probeCount = this.portProbeCount.get(portNumber)
                         .getAndIncrement();
-                OFPortDesc port = findPort(this.sw.getPorts(), portNumber);
+                OFPortDesc port = findPort(portNumber);
                 if (probeCount < LinkDiscovery.MAX_PROBE_COUNT) {
                     this.log.debug("sending fast probe to port");
 
@@ -368,7 +363,7 @@
                 if (this.slowIterator.hasNext()) {
                     final int portNumber = this.slowIterator.next();
                     this.log.debug("sending slow probe to port {}", portNumber);
-                    OFPortDesc port = findPort(this.sw.getPorts(), portNumber);
+                    OFPortDesc port = findPort(portNumber);
 
                     OFPacketOut pkt = this.createLLDPPacketOut(port);
                     this.sendMsg(pkt);
diff --git a/providers/of/link/src/main/java/org/onlab/onos/provider/of/link/impl/OpenFlowLinkProvider.java b/providers/of/link/src/main/java/org/onlab/onos/provider/of/link/impl/OpenFlowLinkProvider.java
index 737ef00..af0bb65 100644
--- a/providers/of/link/src/main/java/org/onlab/onos/provider/of/link/impl/OpenFlowLinkProvider.java
+++ b/providers/of/link/src/main/java/org/onlab/onos/provider/of/link/impl/OpenFlowLinkProvider.java
@@ -87,7 +87,7 @@
             if (ld == null) {
                 return;
             }
-            ld.handleLLDP(pktCtx.parsed(),
+            ld.handleLLDP(pktCtx.unparsed(),
                     pktCtx.inPort());
 
         }
diff --git a/utils/misc/src/main/java/org/onlab/packet/ONLabLddp.java b/utils/misc/src/main/java/org/onlab/packet/ONLabLddp.java
index 520d670..a58387b 100644
--- a/utils/misc/src/main/java/org/onlab/packet/ONLabLddp.java
+++ b/utils/misc/src/main/java/org/onlab/packet/ONLabLddp.java
@@ -30,7 +30,6 @@
  * Refer to IEEE Std 802.1ABTM-2009 for more information.
  *
  */
-@SuppressWarnings("rawtypes")
 public class ONLabLddp extends LLDP {
 
     private static final Logger log = LoggerFactory.getLogger(ONLabLddp.class);
@@ -308,28 +307,34 @@
      * @param packet
      * @return
      */
-    public static boolean isOVXLLDP(byte[] packet) {
+    public static short isOVXLLDP(byte[] packet) {
         if (packet.length < OVX_LLDP_SIZE) {
-            return false;
+            return -1;
         }
 
         // Extra offset due to VLAN tag
         final ByteBuffer bb = ByteBuffer.wrap(packet);
         int offset = 0;
-        if (bb.getShort(ETHERTYPE_OFFSET) != Ethernet.TYPE_LLDP
-                && bb.getShort(ETHERTYPE_OFFSET) != Ethernet.TYPE_BSN) {
+        short ethType = bb.getShort(ETHERTYPE_OFFSET);
+        if (ethType != Ethernet.TYPE_LLDP
+                && ethType != Ethernet.TYPE_BSN) {
             offset = 4;
+            ethType = bb.getShort(ETHERTYPE_OFFSET + offset);
+            if (ethType != Ethernet.TYPE_LLDP
+                    && ethType != Ethernet.TYPE_BSN) {
+                return -1;
+            }
         }
 
         // Compare packet's organizationally specific TLVs to the expected
         // values
         for (int i = 0; i < OUI_TLV.length; i++) {
             if (packet[NAME_TLV_OFFSET + offset + i] != OUI_TLV[i]) {
-                return false;
+                return -1;
             }
         }
 
-        return true;
+        return ethType;
     }
 
     /**