Support for ipv4 dhcp multi server

Change-Id: I77ccf2430f875908423c3d00dbc67517034185fd
(cherry picked from commit 0a856fa12a0e3f1e54898e212f864c207b770671)
diff --git a/apps/dhcprelay/src/main/java/org/onosproject/dhcprelay/Dhcp4HandlerImpl.java b/apps/dhcprelay/src/main/java/org/onosproject/dhcprelay/Dhcp4HandlerImpl.java
index 5b255ab..781a0e2 100644
--- a/apps/dhcprelay/src/main/java/org/onosproject/dhcprelay/Dhcp4HandlerImpl.java
+++ b/apps/dhcprelay/src/main/java/org/onosproject/dhcprelay/Dhcp4HandlerImpl.java
@@ -94,6 +94,7 @@
 import java.util.Collection;
 import java.util.Collections;
 import java.util.List;
+import java.util.ArrayList;
 import java.util.Optional;
 import java.util.Set;
 import java.util.concurrent.atomic.AtomicInteger;
@@ -109,6 +110,8 @@
 import static org.onosproject.net.flowobjective.Objective.Operation.ADD;
 import static org.onosproject.net.flowobjective.Objective.Operation.REMOVE;
 
+import org.onosproject.dhcprelay.Dhcp4HandlerUtil.InternalPacket;
+
 @Component
 @Service
 @Property(name = "version", value = "4")
@@ -172,6 +175,7 @@
 
     private List<DhcpServerInfo> defaultServerInfoList = Lists.newArrayList();
     private List<DhcpServerInfo> indirectServerInfoList = Lists.newArrayList();
+    private Dhcp4HandlerUtil dhcp4HandlerUtil = new Dhcp4HandlerUtil();
 
     @Activate
     protected void activate() {
@@ -260,18 +264,22 @@
             return;
         }
 
-        // TODO: currently we pick up first DHCP server config.
-        // Will use other server configs in the future for HA.
-        DhcpServerConfig serverConfig = configs.iterator().next();
-
-        if (!serverConfig.getDhcpServerIp4().isPresent()) {
-            // not a DHCPv4 config
-            return;
+        Boolean isConfigValid = false;
+        for (DhcpServerConfig serverConfig : configs) {
+            if (serverConfig.getDhcpServerIp4().isPresent()) {
+                isConfigValid = true;
+                break;
+            }
         }
-
-        if (!serverInfoList.isEmpty()) {
+        if (!isConfigValid) {
+            log.warn("No IP V4 server address found.");
+            return;  // No IP V6 address found
+        }
+        // if (!serverInfoList.isEmpty()) {
+        for (DhcpServerInfo oldServerInfo : serverInfoList) {
+            log.info("In for (DhcpServerInfo oldServerInfo : serverInfoList) {");
             // remove old server info
-            DhcpServerInfo oldServerInfo = serverInfoList.remove(0);
+            //DhcpServerInfo oldServerInfo = serverInfoList.remove(0);
 
             // stop monitoring gateway or server
             oldServerInfo.getDhcpGatewayIp4().ifPresent(gatewayIp -> {
@@ -284,43 +292,48 @@
         }
 
         // Create new server info according to the config
-        DhcpServerInfo newServerInfo = new DhcpServerInfo(serverConfig,
-                                                          DhcpServerInfo.Version.DHCP_V4);
-        checkState(newServerInfo.getDhcpServerConnectPoint().isPresent(),
-                   "Connect point not exists");
-        checkState(newServerInfo.getDhcpServerIp4().isPresent(),
-                   "IP of DHCP server not exists");
+        serverInfoList.clear();
+        for (DhcpServerConfig serverConfig : configs) {
+            log.info("// Create new server info according to the config");
+            DhcpServerInfo newServerInfo = new DhcpServerInfo(serverConfig,
+                    DhcpServerInfo.Version.DHCP_V4);
+            checkState(newServerInfo.getDhcpServerConnectPoint().isPresent(),
+                    "Connect point not exists");
+            checkState(newServerInfo.getDhcpServerIp4().isPresent(),
+                    "IP of DHCP server not exists");
 
-        log.debug("DHCP server connect point: {}", newServerInfo.getDhcpServerConnectPoint().orElse(null));
-        log.debug("DHCP server IP: {}", newServerInfo.getDhcpServerIp4().orElse(null));
+            log.debug("DHCP server connect point: {}", newServerInfo.getDhcpServerConnectPoint().orElse(null));
+            log.debug("DHCP server IP: {}", newServerInfo.getDhcpServerIp4().orElse(null));
 
-        Ip4Address serverIp = newServerInfo.getDhcpServerIp4().get();
-        Ip4Address ipToProbe;
-        if (newServerInfo.getDhcpGatewayIp4().isPresent()) {
-            ipToProbe = newServerInfo.getDhcpGatewayIp4().get();
-        } else {
-            ipToProbe = newServerInfo.getDhcpServerIp4().orElse(null);
+            Ip4Address serverIp = newServerInfo.getDhcpServerIp4().get();
+            Ip4Address ipToProbe;
+            if (newServerInfo.getDhcpGatewayIp4().isPresent()) {
+                ipToProbe = newServerInfo.getDhcpGatewayIp4().get();
+            } else {
+                ipToProbe = newServerInfo.getDhcpServerIp4().orElse(null);
+            }
+            log.info("Probe_IP {}", ipToProbe);
+            String hostToProbe = newServerInfo.getDhcpGatewayIp4()
+                    .map(ip -> "gateway").orElse("server");
+
+            log.debug("Probing to resolve {} IP {}", hostToProbe, ipToProbe);
+            hostService.startMonitoringIp(ipToProbe);
+
+            Set<Host> hosts = hostService.getHostsByIp(ipToProbe);
+            if (!hosts.isEmpty()) {
+                Host host = hosts.iterator().next();
+                newServerInfo.setDhcpConnectVlan(host.vlan());
+                newServerInfo.setDhcpConnectMac(host.mac());
+            }
+
+            // Add new server info
+            synchronized (this) {
+                //serverInfoList.clear();
+                serverInfoList.add(newServerInfo);
+            }
+
+            requestDhcpPacket(serverIp);
         }
-        String hostToProbe = newServerInfo.getDhcpGatewayIp4()
-                .map(ip -> "gateway").orElse("server");
-
-        log.debug("Probing to resolve {} IP {}", hostToProbe, ipToProbe);
-        hostService.startMonitoringIp(ipToProbe);
-
-        Set<Host> hosts = hostService.getHostsByIp(ipToProbe);
-        if (!hosts.isEmpty()) {
-            Host host = hosts.iterator().next();
-            newServerInfo.setDhcpConnectVlan(host.vlan());
-            newServerInfo.setDhcpConnectMac(host.mac());
-        }
-
-        // Add new server info
-        synchronized (this) {
-            serverInfoList.clear();
-            serverInfoList.add(0, newServerInfo);
-        }
-
-        requestDhcpPacket(serverIp);
     }
 
     @Override
@@ -343,20 +356,27 @@
                 .findFirst()
                 .orElse(null);
         checkNotNull(incomingPacketType, "Can't get message type from DHCP payload {}", dhcpPayload);
+        Set<Interface> receivingInterfaces = interfaceService.getInterfacesByPort(inPort);
+        //ignore the packets if dhcp client interface is not configured on onos.
+        if (receivingInterfaces.isEmpty()) {
+            log.warn("Virtual interface is not configured on {}", inPort);
+            return;
+        }
         switch (incomingPacketType) {
             case DHCPDISCOVER:
                 // Add the gateway IP as virtual interface IP for server to understand
                 // the lease to be assigned and forward the packet to dhcp server.
-                Ethernet ethernetPacketDiscover =
-                        processDhcpPacketFromClient(context, packet);
-                if (ethernetPacketDiscover != null) {
+                List<InternalPacket> ethernetClientPacket =
+                        processDhcpPacketFromClient(context, packet, receivingInterfaces);
+                for (InternalPacket internalPacket : ethernetClientPacket) {
+                    log.debug("DHCPDISCOVER from {} Forward to server", inPort);
                     writeRequestDhcpRecord(inPort, packet, dhcpPayload);
-                    handleDhcpDiscoverAndRequest(ethernetPacketDiscover, dhcpPayload);
+                    forwardPacket(internalPacket);
                 }
                 break;
             case DHCPOFFER:
                 //reply to dhcp client.
-                Ethernet ethernetPacketOffer = processDhcpPacketFromServer(packet);
+                Ethernet ethernetPacketOffer = processDhcpPacketFromServer(context, packet);
                 if (ethernetPacketOffer != null) {
                     writeResponseDhcpRecord(ethernetPacketOffer, dhcpPayload);
                     sendResponseToClient(ethernetPacketOffer, dhcpPayload);
@@ -365,18 +385,19 @@
             case DHCPREQUEST:
                 // add the gateway ip as virtual interface ip for server to understand
                 // the lease to be assigned and forward the packet to dhcp server.
-                Ethernet ethernetPacketRequest =
-                        processDhcpPacketFromClient(context, packet);
-                if (ethernetPacketRequest != null) {
+                List<InternalPacket> ethernetPacketRequest =
+                        processDhcpPacketFromClient(context, packet, receivingInterfaces);
+                for (InternalPacket internalPacket : ethernetPacketRequest) {
+                    log.debug("DHCPDISCOVER from {} Forward to server", inPort);
                     writeRequestDhcpRecord(inPort, packet, dhcpPayload);
-                    handleDhcpDiscoverAndRequest(ethernetPacketRequest, dhcpPayload);
+                    forwardPacket(internalPacket);
                 }
                 break;
             case DHCPDECLINE:
                 break;
             case DHCPACK:
                 // reply to dhcp client.
-                Ethernet ethernetPacketAck = processDhcpPacketFromServer(packet);
+                Ethernet ethernetPacketAck = processDhcpPacketFromServer(context, packet);
                 if (ethernetPacketAck != null) {
                     writeResponseDhcpRecord(ethernetPacketAck, dhcpPayload);
                     handleDhcpAck(ethernetPacketAck, dhcpPayload);
@@ -574,9 +595,12 @@
 
             // do a basic routing of the packet (this is unicast routing
             // not a relay operation like for other broadcast dhcp packets
-            Ethernet ethernetPacketLQ = processLeaseQueryFromAgent(context, packet);
+            List<InternalPacket> ethernetPacketRequest = processLeaseQueryFromAgent(context, packet);
             // and forward to server
-            handleDhcpDiscoverAndRequest(ethernetPacketLQ, dhcpPayload);
+            for (InternalPacket internalPacket : ethernetPacketRequest) {
+                log.debug("LeaseQueryMsg forward to server");
+                forwardPacket(internalPacket);
+            }
         } else {
             log.warn("LQ: Error! - DHCP relay record for that client not found - ignoring LQ!");
         }
@@ -645,43 +669,29 @@
      * @param ethernetPacket the ethernet payload to process
      * @return processed packet
      */
-    private Ethernet processDhcpPacketFromClient(PacketContext context,
-                                                 Ethernet ethernetPacket) {
+    private List<InternalPacket> processDhcpPacketFromClient(PacketContext context,
+                                                             Ethernet ethernetPacket,
+                                                             Set<Interface> clientInterfaces) {
         ConnectPoint receivedFrom = context.inPacket().receivedFrom();
         DeviceId receivedFromDevice = receivedFrom.deviceId();
+        Ip4Address relayAgentIp = null;
+        relayAgentIp = dhcp4HandlerUtil.getRelayAgentIPv4Address(clientInterfaces);
+        MacAddress relayAgentMac = clientInterfaces.iterator().next().mac();
+        if (relayAgentIp == null || relayAgentMac == null) {
+            log.warn("Missing DHCP relay agent interface Ipv4 addr config for "
+                            + "packet from client on port: {}. Aborting packet processing",
+                    clientInterfaces.iterator().next().connectPoint());
+            return null;
+        }
+        log.debug("Multi DHCP V4 processDhcpPacketFromClient on port {}",
+                   clientInterfaces.iterator().next().connectPoint());
 
         // get dhcp header.
-        Ethernet etherReply = ethernetPacket.duplicate();
+        Ethernet etherReply = (Ethernet) ethernetPacket.clone();
         IPv4 ipv4Packet = (IPv4) etherReply.getPayload();
         UDP udpPacket = (UDP) ipv4Packet.getPayload();
         DHCP dhcpPacket = (DHCP) udpPacket.getPayload();
 
-        // TODO: refactor
-        VlanId dhcpConnectVlan = null;
-        MacAddress dhcpConnectMac = null;
-        Ip4Address dhcpServerIp = null;
-        Ip4Address relayAgentIp = null;
-
-        VlanId indirectDhcpConnectVlan = null;
-        MacAddress indirectDhcpConnectMac = null;
-        Ip4Address indirectDhcpServerIp = null;
-        Ip4Address indirectRelayAgentIp = null;
-
-        if (!defaultServerInfoList.isEmpty()) {
-            DhcpServerInfo serverInfo = defaultServerInfoList.get(0);
-            dhcpConnectVlan = serverInfo.getDhcpConnectVlan().orElse(null);
-            dhcpConnectMac = serverInfo.getDhcpConnectMac().orElse(null);
-            dhcpServerIp = serverInfo.getDhcpServerIp4().orElse(null);
-            relayAgentIp = serverInfo.getRelayAgentIp4(receivedFromDevice).orElse(null);
-        }
-
-        if (!indirectServerInfoList.isEmpty()) {
-            DhcpServerInfo indirectServerInfo = indirectServerInfoList.get(0);
-            indirectDhcpConnectVlan = indirectServerInfo.getDhcpConnectVlan().orElse(null);
-            indirectDhcpConnectMac = indirectServerInfo.getDhcpConnectMac().orElse(null);
-            indirectDhcpServerIp = indirectServerInfo.getDhcpServerIp4().orElse(null);
-            indirectRelayAgentIp = indirectServerInfo.getRelayAgentIp4(receivedFromDevice).orElse(null);
-        }
 
         Ip4Address clientInterfaceIp =
                 interfaceService.getInterfacesByPort(context.inPacket().receivedFrom())
@@ -695,112 +705,149 @@
                         .orElse(null);
         if (clientInterfaceIp == null) {
             log.warn("Can't find interface IP for client interface for port {}",
-                     context.inPacket().receivedFrom());
+                    context.inPacket().receivedFrom());
             return null;
         }
+
         boolean isDirectlyConnected = directlyConnected(dhcpPacket);
-        Interface serverInterface;
-        if (isDirectlyConnected) {
-            serverInterface = getDefaultServerInterface();
-        } else {
-            serverInterface = getIndirectServerInterface();
+        boolean directConnFlag = directlyConnected(dhcpPacket);
+
+        // Multi DHCP Start
+        ConnectPoint clientConnectionPoint = context.inPacket().receivedFrom();
+        VlanId vlanIdInUse = VlanId.vlanId(ethernetPacket.getVlanID());
+        Interface clientInterface = interfaceService.getInterfacesByPort(clientConnectionPoint)
+                .stream().filter(iface -> dhcp4HandlerUtil.interfaceContainsVlan(iface, vlanIdInUse))
+                .findFirst()
+                .orElse(null);
+
+        List<InternalPacket> internalPackets = new ArrayList<>();
+        List<DhcpServerInfo> serverInfoList = findValidServerInfo(directConnFlag);
+        List<DhcpServerInfo> copyServerInfoList = new ArrayList<DhcpServerInfo>(serverInfoList);
+
+
+        for (DhcpServerInfo serverInfo : copyServerInfoList) {
+            etherReply = (Ethernet) ethernetPacket.clone();
+             ipv4Packet = (IPv4) etherReply.getPayload();
+             udpPacket = (UDP) ipv4Packet.getPayload();
+             dhcpPacket = (DHCP) udpPacket.getPayload();
+            if (!checkDhcpServerConnPt(directConnFlag, serverInfo)) {
+                log.warn("Can't get server connect point, ignore");
+                continue;
+            }
+            DhcpServerInfo newServerInfo = getHostInfoForServerInfo(serverInfo, serverInfoList);
+            if (newServerInfo == null) {
+                log.warn("Can't get server interface with host info resolved, ignore");
+                continue;
+            }
+
+            Interface serverInterface = getServerInterface(newServerInfo);
             if (serverInterface == null) {
-                // Indirect server interface not found, use default server interface
-                serverInterface = getDefaultServerInterface();
+                log.warn("Can't get server interface, ignore");
+                continue;
             }
-        }
-        if (serverInterface == null) {
-            log.warn("Can't get {} server interface, ignore", isDirectlyConnected ? "direct" : "indirect");
-            return null;
-        }
-        Ip4Address ipFacingServer = getFirstIpFromInterface(serverInterface);
-        MacAddress macFacingServer = serverInterface.mac();
-        if (ipFacingServer == null || macFacingServer == null) {
-            log.warn("No IP address for server Interface {}", serverInterface);
-            return null;
-        }
-        if (dhcpConnectMac == null) {
-            log.warn("DHCP Server/Gateway IP not yet resolved .. Aborting DHCP "
-                             + "packet processing from client on port: {}",
-                     context.inPacket().receivedFrom());
-            return null;
-        }
 
-        etherReply.setSourceMACAddress(macFacingServer);
-        ipv4Packet.setSourceAddress(ipFacingServer.toInt());
+            Ip4Address ipFacingServer = getFirstIpFromInterface(serverInterface);
+            MacAddress macFacingServer = serverInterface.mac();
+            log.debug("Interfacing server {} Mac : {} ", ipFacingServer, macFacingServer);
+            if (ipFacingServer == null || macFacingServer == null) {
+                log.warn("No IP address for server Interface {}", serverInterface);
+                //return null;
+                continue;
+            }
 
-        if (isDirectlyConnected) {
-            etherReply.setDestinationMACAddress(dhcpConnectMac);
-            etherReply.setVlanID(dhcpConnectVlan.toShort());
-            ipv4Packet.setDestinationAddress(dhcpServerIp.toInt());
 
-            ConnectPoint inPort = context.inPacket().receivedFrom();
-            VlanId vlanId = VlanId.vlanId(ethernetPacket.getVlanID());
-            // add connected in port and vlan
-            CircuitId cid = new CircuitId(inPort.toString(), vlanId);
-            byte[] circuitId = cid.serialize();
-            DhcpOption circuitIdSubOpt = new DhcpOption();
-            circuitIdSubOpt
-                    .setCode(CIRCUIT_ID.getValue())
-                    .setLength((byte) circuitId.length)
-                    .setData(circuitId);
+            etherReply.setSourceMACAddress(macFacingServer);
+            // set default info and replace with indirect if available later on.
+            if (newServerInfo.getDhcpConnectMac().isPresent()) {
+                etherReply.setDestinationMACAddress(newServerInfo.getDhcpConnectMac().get());
+            }
+            if (newServerInfo.getDhcpConnectVlan().isPresent()) {
+                etherReply.setVlanID(newServerInfo.getDhcpConnectVlan().get().toShort());
+            }
+            ipv4Packet.setSourceAddress(ipFacingServer.toInt());
+            ipv4Packet.setDestinationAddress(newServerInfo.getDhcpServerIp4().get().toInt());
+            log.info("Directly connected {}", isDirectlyConnected);
+            log.info("Dhcp Server IP: {}", newServerInfo.getDhcpServerIp4().get());
+            if (isDirectlyConnected) {
 
-            DhcpRelayAgentOption newRelayAgentOpt = new DhcpRelayAgentOption();
-            newRelayAgentOpt.setCode(OptionCode_CircuitID.getValue());
-            newRelayAgentOpt.addSubOption(circuitIdSubOpt);
-
-            // Removes END option first
-            List<DhcpOption> options = dhcpPacket.getOptions().stream()
-                    .filter(opt -> opt.getCode() != OptionCode_END.getValue())
-                    .collect(Collectors.toList());
-
-            // push relay agent option
-            options.add(newRelayAgentOpt);
-
-            // make sure option 255(End) is the last option
-            DhcpOption endOption = new DhcpOption();
-            endOption.setCode(OptionCode_END.getValue());
-            options.add(endOption);
-
-            dhcpPacket.setOptions(options);
-
-            // Sets relay agent IP
-            int effectiveRelayAgentIp = relayAgentIp != null ?
-                    relayAgentIp.toInt() : clientInterfaceIp.toInt();
-            dhcpPacket.setGatewayIPAddress(effectiveRelayAgentIp);
-        } else {
-            if (indirectDhcpServerIp != null) {
-                // Use indirect server config for indirect packets if configured
-                etherReply.setDestinationMACAddress(indirectDhcpConnectMac);
-                etherReply.setVlanID(indirectDhcpConnectVlan.toShort());
-                ipv4Packet.setDestinationAddress(indirectDhcpServerIp.toInt());
-
-                // Set giaddr if indirect relay agent IP is configured
-                if (indirectRelayAgentIp != null) {
-                    dhcpPacket.setGatewayIPAddress(indirectRelayAgentIp.toInt());
+                log.info("**Default****Dhcp Server IP: {}", newServerInfo.getDhcpServerIp4().get());
+                if (newServerInfo.getDhcpConnectMac().isPresent()) {
+                    etherReply.setDestinationMACAddress(newServerInfo.getDhcpConnectMac().get());
                 }
+                if (newServerInfo.getDhcpConnectVlan().isPresent()) {
+                    etherReply.setVlanID(newServerInfo.getDhcpConnectVlan().get().toShort());
+                }
+
+                ipv4Packet.setDestinationAddress(newServerInfo.getDhcpServerIp4().get().toInt());
+
+
+                ConnectPoint inPort = context.inPacket().receivedFrom();
+                VlanId vlanId = VlanId.vlanId(ethernetPacket.getVlanID());
+                // add connected in port and vlan
+                CircuitId cid = new CircuitId(inPort.toString(), vlanId);
+                byte[] circuitId = cid.serialize();
+                DhcpOption circuitIdSubOpt = new DhcpOption();
+                circuitIdSubOpt
+                        .setCode(CIRCUIT_ID.getValue())
+                        .setLength((byte) circuitId.length)
+                        .setData(circuitId);
+
+                DhcpRelayAgentOption newRelayAgentOpt = new DhcpRelayAgentOption();
+                newRelayAgentOpt.setCode(OptionCode_CircuitID.getValue());
+                newRelayAgentOpt.addSubOption(circuitIdSubOpt);
+
+                // Removes END option first
+                List<DhcpOption> options = dhcpPacket.getOptions().stream()
+                        .filter(opt -> opt.getCode() != OptionCode_END.getValue())
+                        .collect(Collectors.toList());
+
+                // push relay agent option
+                options.add(newRelayAgentOpt);
+
+                // make sure option 255(End) is the last option
+                DhcpOption endOption = new DhcpOption();
+                endOption.setCode(OptionCode_END.getValue());
+                options.add(endOption);
+
+                dhcpPacket.setOptions(options);
+
+                relayAgentIp = serverInfo.getRelayAgentIp4(receivedFromDevice).orElse(null);
+
+                // Sets relay agent IP
+                int effectiveRelayAgentIp = relayAgentIp != null ?
+                        relayAgentIp.toInt() : clientInterfaceIp.toInt();
+                dhcpPacket.setGatewayIPAddress(effectiveRelayAgentIp);
+                log.info("In Default, Relay Agent IP {}", effectiveRelayAgentIp);
             } else {
-                // Otherwise, use default server config for indirect packets
-                etherReply.setDestinationMACAddress(dhcpConnectMac);
-                etherReply.setVlanID(dhcpConnectVlan.toShort());
-                ipv4Packet.setDestinationAddress(dhcpServerIp.toInt());
-
-                // Set giaddr if direct relay agent IP is configured
-                if (relayAgentIp != null) {
-                    dhcpPacket.setGatewayIPAddress(relayAgentIp.toInt());
+                if (!newServerInfo.getDhcpServerIp4().isPresent()) {
+                  // do nothing
+                } else if (!newServerInfo.getDhcpConnectMac().isPresent()) {
+                    continue;
+                } else {
+                    relayAgentIp = newServerInfo.getRelayAgentIp4(receivedFromDevice).orElse(null);
+                    // Sets relay agent IP
+                    int effectiveRelayAgentIp = relayAgentIp != null ?
+                            relayAgentIp.toInt() : clientInterfaceIp.toInt();
+                    dhcpPacket.setGatewayIPAddress(effectiveRelayAgentIp);
                 }
             }
-        }
 
-        udpPacket.setPayload(dhcpPacket);
-        // As a DHCP relay, the source port should be server port( instead
-        // of client port.
-        udpPacket.setSourcePort(UDP.DHCP_SERVER_PORT);
-        udpPacket.setDestinationPort(UDP.DHCP_SERVER_PORT);
-        ipv4Packet.setPayload(udpPacket);
-        ipv4Packet.setTtl((byte) 64);
-        etherReply.setPayload(ipv4Packet);
-        return etherReply;
+            // Remove broadcast flag
+            dhcpPacket.setFlags((short) 0);
+
+            udpPacket.setPayload(dhcpPacket);
+            // As a DHCP relay, the source port should be server port( instead
+            // of client port.
+            udpPacket.setSourcePort(UDP.DHCP_SERVER_PORT);
+            udpPacket.setDestinationPort(UDP.DHCP_SERVER_PORT);
+            ipv4Packet.setPayload(udpPacket);
+            ipv4Packet.setTtl((byte) 64);
+            etherReply.setPayload(ipv4Packet);
+            InternalPacket internalPacket = new Dhcp4HandlerUtil().new InternalPacket(etherReply,
+                    serverInfo.getDhcpServerConnectPoint().get());
+            internalPackets.add(internalPacket);
+        }
+        return internalPackets;
     }
 
 
@@ -811,14 +858,20 @@
      * @param ethernetPacket the ethernet payload to process
      * @return processed packet
      */
-    private Ethernet processLeaseQueryFromAgent(PacketContext context,
-                                                Ethernet ethernetPacket) {
+    private List<InternalPacket> processLeaseQueryFromAgent(PacketContext context,
+                                                            Ethernet ethernetPacket) {
+        ConnectPoint receivedFrom = context.inPacket().receivedFrom();
+        DeviceId receivedFromDevice = receivedFrom.deviceId();
+
         // get dhcp header.
         Ethernet etherReply = (Ethernet) ethernetPacket.clone();
         IPv4 ipv4Packet = (IPv4) etherReply.getPayload();
         UDP udpPacket = (UDP) ipv4Packet.getPayload();
         DHCP dhcpPacket = (DHCP) udpPacket.getPayload();
 
+        Ip4Address relayAgentIp = null;
+
+
         VlanId dhcpConnectVlan = null;
         MacAddress dhcpConnectMac = null;
         Ip4Address dhcpServerIp = null;
@@ -827,20 +880,6 @@
         MacAddress indirectDhcpConnectMac = null;
         Ip4Address indirectDhcpServerIp = null;
 
-        if (!defaultServerInfoList.isEmpty()) {
-            DhcpServerInfo serverInfo = defaultServerInfoList.get(0);
-            dhcpConnectVlan = serverInfo.getDhcpConnectVlan().orElse(null);
-            dhcpConnectMac = serverInfo.getDhcpConnectMac().orElse(null);
-            dhcpServerIp = serverInfo.getDhcpServerIp4().orElse(null);
-        }
-
-        if (!indirectServerInfoList.isEmpty()) {
-            DhcpServerInfo indirectServerInfo = indirectServerInfoList.get(0);
-            indirectDhcpConnectVlan = indirectServerInfo.getDhcpConnectVlan().orElse(null);
-            indirectDhcpConnectMac = indirectServerInfo.getDhcpConnectMac().orElse(null);
-            indirectDhcpServerIp = indirectServerInfo.getDhcpServerIp4().orElse(null);
-        }
-
         Ip4Address clientInterfaceIp =
                 interfaceService.getInterfacesByPort(context.inPacket().receivedFrom())
                         .stream()
@@ -853,58 +892,109 @@
                         .orElse(null);
         if (clientInterfaceIp == null) {
             log.warn("Can't find interface IP for client interface for port {}",
-                     context.inPacket().receivedFrom());
+                    context.inPacket().receivedFrom());
             return null;
         }
+
         boolean isDirectlyConnected = directlyConnected(dhcpPacket);
-        Interface serverInterface;
-        if (isDirectlyConnected) {
-            serverInterface = getDefaultServerInterface();
-        } else {
-            serverInterface = getIndirectServerInterface();
-            if (serverInterface == null) {
-                // Indirect server interface not found, use default server interface
-                serverInterface = getDefaultServerInterface();
+        boolean directConnFlag = directlyConnected(dhcpPacket);
+
+        // Multi DHCP Start
+        ConnectPoint clientConnectionPoint = context.inPacket().receivedFrom();
+        VlanId vlanIdInUse = VlanId.vlanId(ethernetPacket.getVlanID());
+        Interface clientInterface = interfaceService.getInterfacesByPort(clientConnectionPoint)
+                .stream().filter(iface -> dhcp4HandlerUtil.interfaceContainsVlan(iface, vlanIdInUse))
+                .findFirst()
+                .orElse(null);
+
+        List<InternalPacket> internalPackets = new ArrayList<>();
+        List<DhcpServerInfo> serverInfoList = findValidServerInfo(directConnFlag);
+        List<DhcpServerInfo> copyServerInfoList = new ArrayList<DhcpServerInfo>(serverInfoList);
+
+        for (DhcpServerInfo serverInfo : copyServerInfoList) {
+             // get dhcp header.
+             etherReply = (Ethernet) ethernetPacket.clone();
+             ipv4Packet = (IPv4) etherReply.getPayload();
+             udpPacket = (UDP) ipv4Packet.getPayload();
+             dhcpPacket = (DHCP) udpPacket.getPayload();
+
+            if (!checkDhcpServerConnPt(directConnFlag, serverInfo)) {
+                log.warn("Can't get server connect point, ignore");
+                continue;
             }
-        }
-        if (serverInterface == null) {
-            log.warn("Can't get {} server interface, ignore", isDirectlyConnected ? "direct" : "indirect");
-            return null;
-        }
-        Ip4Address ipFacingServer = getFirstIpFromInterface(serverInterface);
-        MacAddress macFacingServer = serverInterface.mac();
-        if (ipFacingServer == null || macFacingServer == null) {
-            log.warn("No IP address for server Interface {}", serverInterface);
-            return null;
-        }
-        if (dhcpConnectMac == null) {
-            log.warn("DHCP server/gateway not yet resolved .. Aborting DHCP "
-                             + "packet processing from client on port: {}",
-                     context.inPacket().receivedFrom());
-            return null;
-        }
+            DhcpServerInfo newServerInfo = getHostInfoForServerInfo(serverInfo, serverInfoList);
+            if (newServerInfo == null) {
+                log.warn("Can't get server interface with host info resolved, ignore");
+                continue;
+            }
 
-        etherReply.setSourceMACAddress(macFacingServer);
-        etherReply.setDestinationMACAddress(dhcpConnectMac);
-        etherReply.setVlanID(dhcpConnectVlan.toShort());
-        ipv4Packet.setSourceAddress(ipFacingServer.toInt());
-        ipv4Packet.setDestinationAddress(dhcpServerIp.toInt());
+            Interface serverInterface = getServerInterface(newServerInfo);
+            if (serverInterface == null) {
+                log.warn("Can't get server interface, ignore");
+                continue;
+            }
+            Ip4Address ipFacingServer = getFirstIpFromInterface(serverInterface);
+            MacAddress macFacingServer = serverInterface.mac();
+            if (ipFacingServer == null || macFacingServer == null) {
+                log.warn("No IP address for server Interface {}", serverInterface);
+                continue;
+            }
 
-        if (indirectDhcpServerIp != null) {
-            // Indirect case, replace destination to indirect dhcp server if exist
-            etherReply.setDestinationMACAddress(indirectDhcpConnectMac);
-            etherReply.setVlanID(indirectDhcpConnectVlan.toShort());
-            ipv4Packet.setDestinationAddress(indirectDhcpServerIp.toInt());
+            etherReply.setSourceMACAddress(macFacingServer);
+            etherReply.setDestinationMACAddress(dhcpConnectMac);
+            etherReply.setVlanID(dhcpConnectVlan.toShort());
+            ipv4Packet.setSourceAddress(ipFacingServer.toInt());
+            ipv4Packet.setDestinationAddress(dhcpServerIp.toInt());
+            if (isDirectlyConnected) {
+                // set default info and replace with indirect if available later on.
+                if (newServerInfo.getDhcpConnectMac().isPresent()) {
+                    etherReply.setDestinationMACAddress(newServerInfo.getDhcpConnectMac().get());
+                }
+                if (newServerInfo.getDhcpConnectVlan().isPresent()) {
+                    etherReply.setVlanID(serverInfo.getDhcpConnectVlan().get().toShort());
+                }
+                relayAgentIp = newServerInfo.getRelayAgentIp4(receivedFromDevice).orElse(null);
+                // Sets relay agent IP
+                int effectiveRelayAgentIp = relayAgentIp != null ?
+                        relayAgentIp.toInt() : clientInterfaceIp.toInt();
+                dhcpPacket.setGatewayIPAddress(effectiveRelayAgentIp);
+            } else {
+                if (!newServerInfo.getDhcpServerIp4().isPresent()) {
+                  //do nothing
+                } else if (!newServerInfo.getDhcpConnectMac().isPresent()) {
+                    continue;
+                } else {
+                    relayAgentIp = newServerInfo.getRelayAgentIp4(receivedFromDevice).orElse(null);
+                    // Sets relay agent IP
+                    int effectiveRelayAgentIp = relayAgentIp != null ?
+                            relayAgentIp.toInt() : clientInterfaceIp.toInt();
+                    dhcpPacket.setGatewayIPAddress(effectiveRelayAgentIp);
+                    dhcpPacket.setGatewayIPAddress(effectiveRelayAgentIp);
+                    log.info("Relay Agent IP {}", relayAgentIp);
+                }
+
+                log.info("In Direct");
+            }
+
+            // Remove broadcast flag
+            dhcpPacket.setFlags((short) 0);
+
+            udpPacket.setPayload(dhcpPacket);
+            // As a DHCP relay, the source port should be server port( instead
+            // of client port.
+            udpPacket.setSourcePort(UDP.DHCP_SERVER_PORT);
+            udpPacket.setDestinationPort(UDP.DHCP_SERVER_PORT);
+            ipv4Packet.setPayload(udpPacket);
+            ipv4Packet.setTtl((byte) 64);
+            etherReply.setPayload(ipv4Packet);
+            ////return etherReply;
+            Dhcp4HandlerUtil.InternalPacket internalPacket = new Dhcp4HandlerUtil().new InternalPacket(etherReply,
+                    newServerInfo.getDhcpServerConnectPoint().get());
+            internalPackets.add(internalPacket);
         }
+        log.warn("num of processLeaseQueryFromAgent packets to send is{}", internalPackets.size());
 
-        udpPacket.setPayload(dhcpPacket);
-        // As a DHCP relay, the source port should be server port( instead
-        // of client port.
-        udpPacket.setSourcePort(UDP.DHCP_SERVER_PORT);
-        udpPacket.setDestinationPort(UDP.DHCP_SERVER_PORT);
-        ipv4Packet.setPayload(udpPacket);
-        etherReply.setPayload(ipv4Packet);
-        return etherReply;
+        return internalPackets;
     }
 
 
@@ -982,9 +1072,9 @@
      * @param ethernetPacket the original packet comes from server
      * @return new packet which will send to the client
      */
-    private Ethernet processDhcpPacketFromServer(Ethernet ethernetPacket) {
+    private Ethernet processDhcpPacketFromServer(PacketContext context, Ethernet ethernetPacket) {
         // get dhcp header.
-        Ethernet etherReply = ethernetPacket.duplicate();
+        Ethernet etherReply = (Ethernet) ethernetPacket.clone();
         IPv4 ipv4Packet = (IPv4) etherReply.getPayload();
         UDP udpPacket = (UDP) ipv4Packet.getPayload();
         DHCP dhcpPayload = (DHCP) udpPacket.getPayload();
@@ -998,6 +1088,19 @@
             return null;
         }
         VlanId vlanId;
+        ConnectPoint inPort = context.inPacket().receivedFrom();
+        boolean directConnFlag = directlyConnected(dhcpPayload);
+        DhcpServerInfo foundServerInfo = findServerInfoFromServer(directConnFlag, inPort);
+
+        if (foundServerInfo == null) {
+            log.warn("Cannot find server info");
+            return null;
+        } else {
+            if (dhcp4HandlerUtil.isServerIpEmpty(foundServerInfo)) {
+                log.warn("Cannot find server info's ipaddress");
+                return null;
+            }
+        }
         if (clientInterface.vlanTagged().isEmpty()) {
             vlanId = clientInterface.vlan();
         } else {
@@ -1158,7 +1261,7 @@
      * @return Ethernet packet processed
      */
     private Ethernet removeRelayAgentOption(Ethernet ethPacket) {
-        Ethernet ethernet = ethPacket.duplicate();
+        Ethernet ethernet = (Ethernet) ethPacket.duplicate();
         IPv4 ipv4 = (IPv4) ethernet.getPayload();
         UDP udp = (UDP) ipv4.getPayload();
         DHCP dhcpPayload = (DHCP) udp.getPayload();
@@ -1288,34 +1391,6 @@
     }
 
     /**
-     * forward the packet to ConnectPoint where the DHCP server is attached.
-     *
-     * @param packet the packet
-     */
-    private void handleDhcpDiscoverAndRequest(Ethernet packet, DHCP dhcpPayload) {
-        boolean direct = directlyConnected(dhcpPayload);
-        DhcpServerInfo serverInfo = defaultServerInfoList.get(0);
-        if (!direct && !indirectServerInfoList.isEmpty()) {
-            serverInfo = indirectServerInfoList.get(0);
-        }
-        ConnectPoint portToFotward = serverInfo.getDhcpServerConnectPoint().orElse(null);
-        // send packet to dhcp server connect point.
-        if (portToFotward != null) {
-            TrafficTreatment t = DefaultTrafficTreatment.builder()
-                    .setOutput(portToFotward.port()).build();
-            OutboundPacket o = new DefaultOutboundPacket(
-                    portToFotward.deviceId(), t, ByteBuffer.wrap(packet.serialize()));
-            if (log.isTraceEnabled()) {
-                log.trace("Relaying packet to dhcp server {}", packet);
-            }
-            packetService.emit(o);
-        } else {
-            log.warn("Can't find DHCP server connect point, abort.");
-        }
-    }
-
-
-    /**
      * Gets output interface of a dhcp packet.
      * If option 82 exists in the dhcp packet and the option was sent by
      * ONOS (circuit format is correct), use the connect
@@ -1641,4 +1716,135 @@
     public void setDhcpFpmEnabled(Boolean enabled) {
         // v4 does not use fpm. Do nothing.
     }
+    private List<DhcpServerInfo> findValidServerInfo(boolean directConnFlag) {
+        List<DhcpServerInfo> validServerInfo;
+
+        if (directConnFlag || indirectServerInfoList.isEmpty()) {
+            validServerInfo = new ArrayList<DhcpServerInfo>(defaultServerInfoList);
+        } else {
+            validServerInfo = new ArrayList<DhcpServerInfo>(indirectServerInfoList);
+        }
+        return validServerInfo;
+    }
+
+
+    private boolean checkDhcpServerConnPt(boolean directConnFlag,
+                                          DhcpServerInfo serverInfo) {
+        if (serverInfo.getDhcpServerConnectPoint() == null) {
+            log.warn("DHCP4 server connect point for {} connPt {}",
+                    directConnFlag ? "direct" : "indirect", serverInfo.getDhcpServerConnectPoint());
+            return false;
+        }
+        return true;
+    }
+
+    /**
+     * Checks if serverInfo's host info (mac and vlan) is filled in; if not, fills in.
+     *
+     * @param serverInfo server information
+     * @return newServerInfo if host info can be either found or filled in.
+     */
+    private DhcpServerInfo getHostInfoForServerInfo(DhcpServerInfo serverInfo, List<DhcpServerInfo> sererInfoList) {
+        DhcpServerInfo newServerInfo = null;
+        MacAddress  dhcpServerConnectMac = serverInfo.getDhcpConnectMac().orElse(null);
+        VlanId dhcpConnectVlan = serverInfo.getDhcpConnectVlan().orElse(null);
+        ConnectPoint dhcpServerConnectPoint = serverInfo.getDhcpServerConnectPoint().orElse(null);
+
+        if (dhcpServerConnectMac != null && dhcpConnectVlan != null) {
+            newServerInfo = serverInfo;
+            log.warn("DHCP server {} host info found. ConnectPt{}  Mac {} vlan {}", serverInfo.getDhcpServerIp4(),
+                    dhcpServerConnectPoint, dhcpServerConnectMac, dhcpConnectVlan);
+        } else {
+            log.warn("DHCP server {} not resolve yet connectPt {} mac {} vlan {}", serverInfo.getDhcpServerIp4(),
+                    dhcpServerConnectPoint, dhcpServerConnectMac, dhcpConnectVlan);
+
+            Ip4Address ipToProbe;
+            if (serverInfo.getDhcpGatewayIp4().isPresent()) {
+                ipToProbe = serverInfo.getDhcpGatewayIp4().get();
+            } else {
+                ipToProbe = serverInfo.getDhcpServerIp4().orElse(null);
+            }
+            String hostToProbe = serverInfo.getDhcpGatewayIp6()
+                    .map(ip -> "gateway").orElse("server");
+
+            log.warn("Dynamically probing to resolve {} IP {}", hostToProbe, ipToProbe);
+            hostService.startMonitoringIp(ipToProbe);
+
+            Set<Host> hosts = hostService.getHostsByIp(ipToProbe);
+            if (!hosts.isEmpty()) {
+                int serverInfoIndex = sererInfoList.indexOf(serverInfo);
+                Host host = hosts.iterator().next();
+                serverInfo.setDhcpConnectVlan(host.vlan());
+                serverInfo.setDhcpConnectMac(host.mac());
+                // replace the serverInfo in the list
+                sererInfoList.set(serverInfoIndex, serverInfo);
+                newServerInfo = serverInfo;
+                log.warn("Dynamically host found host {}", host);
+            } else {
+                log.warn("No host found host ip {} dynamically", ipToProbe);
+            }
+        }
+        return newServerInfo;
+    }
+
+    /**
+     * Gets Interface facing to the server for default host.
+     *
+     * @param serverInfo server information
+     * @return the Interface facing to the server; null if not found
+     */
+    private Interface getServerInterface(DhcpServerInfo serverInfo) {
+        Interface serverInterface = null;
+
+        ConnectPoint dhcpServerConnectPoint = serverInfo.getDhcpServerConnectPoint().orElse(null);
+        VlanId dhcpConnectVlan = serverInfo.getDhcpConnectVlan().orElse(null);
+
+        if (dhcpServerConnectPoint != null && dhcpConnectVlan != null) {
+            serverInterface = interfaceService.getInterfacesByPort(dhcpServerConnectPoint)
+                    .stream()
+                    .filter(iface -> dhcp4HandlerUtil.interfaceContainsVlan(iface, dhcpConnectVlan))
+                    .findFirst()
+                    .orElse(null);
+        } else {
+            log.warn("DHCP server {} not resolve yet connectPoint {} vlan {}", serverInfo.getDhcpServerIp6(),
+                    dhcpServerConnectPoint, dhcpConnectVlan);
+        }
+
+        return serverInterface;
+    }
+
+    //forward the packet to ConnectPoint where the DHCP server is attached.
+    private void forwardPacket(InternalPacket packet) {
+        //send Packetout to dhcp server connectpoint.
+        if (packet.destLocation != null) {
+            TrafficTreatment t = DefaultTrafficTreatment.builder()
+                    .setOutput(packet.destLocation.port()).build();
+            OutboundPacket o = new DefaultOutboundPacket(
+                    packet.destLocation.deviceId(), t, ByteBuffer.wrap(packet.packet.serialize()));
+            if (log.isTraceEnabled()) {
+                log.trace("Relaying packet to destination {}", packet.destLocation);
+            }
+            log.info("DHCP RELAY: packetService.emit(o) to port {}", packet.destLocation);
+            packetService.emit(o);
+        }
+    }
+
+
+    private DhcpServerInfo findServerInfoFromServer(boolean directConnFlag, ConnectPoint inPort) {
+        List<DhcpServerInfo> validServerInfoList = findValidServerInfo(directConnFlag);
+        DhcpServerInfo  foundServerInfo = null;
+        for (DhcpServerInfo serverInfo : validServerInfoList) {
+            if (inPort.equals(serverInfo.getDhcpServerConnectPoint().get())) {
+                foundServerInfo = serverInfo;
+                log.warn("ServerInfo found for Rcv port {} Server Connect Point {} for {}",
+                        inPort, serverInfo.getDhcpServerConnectPoint(), directConnFlag ? "direct" : "indirect");
+                break;
+            } else {
+                log.warn("Rcv port {} not the same as Server Connect Point {} for {}",
+                        inPort, serverInfo.getDhcpServerConnectPoint(), directConnFlag ? "direct" : "indirect");
+            }
+        }
+        return foundServerInfo;
+    }
+
 }
diff --git a/apps/dhcprelay/src/main/java/org/onosproject/dhcprelay/Dhcp4HandlerUtil.java b/apps/dhcprelay/src/main/java/org/onosproject/dhcprelay/Dhcp4HandlerUtil.java
new file mode 100644
index 0000000..077c6ca
--- /dev/null
+++ b/apps/dhcprelay/src/main/java/org/onosproject/dhcprelay/Dhcp4HandlerUtil.java
@@ -0,0 +1,104 @@
+/*
+ * Copyright 2017-present Open Networking Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+package org.onosproject.dhcprelay;
+
+import org.onlab.packet.Ip4Address;
+import org.onlab.packet.VlanId;
+
+import org.onlab.packet.Ethernet;
+
+import org.onlab.util.HexString;
+import org.onosproject.dhcprelay.api.DhcpServerInfo;
+import org.onosproject.net.ConnectPoint;
+import org.onosproject.net.host.InterfaceIpAddress;
+import org.onosproject.net.intf.Interface;
+
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import java.util.Set;
+
+public class Dhcp4HandlerUtil {
+
+
+    private final Logger log = LoggerFactory.getLogger(getClass());
+
+    /**
+     * Returns the first v4 interface ip out of a set of interfaces or null.
+     *
+     * @param intfs set of interfaces
+     * @return Ip4Address / null if not present
+     */
+    public Ip4Address getRelayAgentIPv4Address(Set<Interface> intfs) {
+        for (Interface intf : intfs) {
+            for (InterfaceIpAddress ip : intf.ipAddressesList()) {
+                Ip4Address relayAgentIp = ip.ipAddress().getIp4Address();
+                if (relayAgentIp != null) {
+                    return relayAgentIp;
+                }
+            }
+        }
+        return null;
+    }
+
+    /**
+     * Determind if an Interface contains a vlan id.
+     *
+     * @param iface the Interface
+     * @param vlanId the vlan id
+     * @return true if the Interface contains the vlan id
+     */
+    public boolean interfaceContainsVlan(Interface iface, VlanId vlanId) {
+        if (vlanId.equals(VlanId.NONE)) {
+            // untagged packet, check if vlan untagged or vlan native is not NONE
+            return !iface.vlanUntagged().equals(VlanId.NONE) ||
+                    !iface.vlanNative().equals(VlanId.NONE);
+        }
+        // tagged packet, check if the interface contains the vlan
+        return iface.vlanTagged().contains(vlanId);
+   }
+
+    /**
+     * Check if a given server info has v6 ipaddress.
+     *
+     * @param serverInfo server info to check
+     * @return true if server info has v6 ip address; false otherwise
+     */
+    public boolean isServerIpEmpty(DhcpServerInfo serverInfo) {
+        if (!serverInfo.getDhcpServerIp4().isPresent()) {
+            log.warn("DhcpServerIp not available, use default DhcpServerIp {}",
+                    HexString.toHexString(serverInfo.getDhcpServerIp4().get().toOctets()));
+            return true;
+        }
+        return false;
+    }
+
+
+    /**
+     * the new class the contains Ethernet packet and destination port.
+     */
+    public class InternalPacket {
+        Ethernet packet;
+        ConnectPoint destLocation;
+        public InternalPacket(Ethernet newPacket, ConnectPoint newLocation) {
+            packet = newPacket;
+            destLocation = newLocation;
+        }
+    }
+
+
+}
+