[SEBA-842] Doubletagging DHCP packets going to VOLTHA

Change-Id: I8e7d2af217228e96dcdc3bc94318b5e29b71f676
diff --git a/internal/bbsim/api/onus_handler.go b/internal/bbsim/api/onus_handler.go
index ce0dba0..1b823d8 100644
--- a/internal/bbsim/api/onus_handler.go
+++ b/internal/bbsim/api/onus_handler.go
@@ -52,7 +52,7 @@
 func (s BBSimServer) GetONU(ctx context.Context, req *bbsim.ONURequest) (*bbsim.ONU, error) {
 	olt := devices.GetOLT()
 
-	onu, err := olt.FindOnu(req.SerialNumber)
+	onu, err := olt.FindOnuBySn(req.SerialNumber)
 
 	if err != nil {
 		res := bbsim.ONU{}
@@ -87,7 +87,7 @@
 
 	olt := devices.GetOLT()
 
-	onu, err := olt.FindOnu(req.SerialNumber)
+	onu, err := olt.FindOnuBySn(req.SerialNumber)
 
 	if err != nil {
 		res.StatusCode = int32(codes.NotFound)
@@ -132,7 +132,7 @@
 
 	olt := devices.GetOLT()
 
-	onu, err := olt.FindOnu(req.SerialNumber)
+	onu, err := olt.FindOnuBySn(req.SerialNumber)
 
 	if err != nil {
 		res.StatusCode = int32(codes.NotFound)
diff --git a/internal/bbsim/devices/olt.go b/internal/bbsim/devices/olt.go
index 2d46ced..864a88e 100644
--- a/internal/bbsim/devices/olt.go
+++ b/internal/bbsim/devices/olt.go
@@ -23,6 +23,7 @@
 	"github.com/google/gopacket"
 	"github.com/google/gopacket/layers"
 	"github.com/looplab/fsm"
+	"github.com/opencord/bbsim/internal/bbsim/packetHandlers"
 	bbsim "github.com/opencord/bbsim/internal/bbsim/types"
 	"github.com/opencord/voltha-protos/go/openolt"
 	"github.com/opencord/voltha-protos/go/tech_profile"
@@ -365,23 +366,58 @@
 	}).Debug("Started NNI Channel")
 	nniId := o.Nnis[0].ID // FIXME we are assuming we have only one NNI
 	for message := range o.nniPktInChannel {
-		oltLogger.Debug("Received packets on NNI Channel")
+		oltLogger.Tracef("Received packets on NNI Channel")
+
+		onuMac, err := packetHandlers.GetDstMacAddressFromPacket(message.Pkt)
+
+		if err != nil {
+			log.WithFields(log.Fields{
+				"IntfType": "nni",
+				"IntfId":   nniId,
+				"Pkt":      message.Pkt.Data(),
+			}).Info("Can't find Dst MacAddress in packet")
+			return
+		}
+
+		onu, err := o.FindOnuByMacAddress(onuMac)
+		if err != nil {
+			log.WithFields(log.Fields{
+				"IntfType":   "nni",
+				"IntfId":     nniId,
+				"Pkt":        message.Pkt.Data(),
+				"MacAddress": onuMac.String(),
+			}).Info("Can't find ONU with MacAddress")
+			return
+		}
+
+		doubleTaggedPkt, err := packetHandlers.PushDoubleTag(onu.STag, onu.CTag, message.Pkt)
+		if err != nil {
+			log.Error("Fail to add double tag to packet")
+		}
+
 		data := &openolt.Indication_PktInd{PktInd: &openolt.PacketIndication{
 			IntfType: "nni",
 			IntfId:   nniId,
-			Pkt:      message.Pkt.Data()}}
+			Pkt:      doubleTaggedPkt.Data()}}
 		if err := stream.Send(&openolt.Indication{Data: data}); err != nil {
 			oltLogger.WithFields(log.Fields{
 				"IntfType": data.PktInd.IntfType,
 				"IntfId":   nniId,
-				"Pkt":      message.Pkt.Data(),
+				"Pkt":      doubleTaggedPkt.Data(),
 			}).Errorf("Fail to send PktInd indication: %v", err)
 		}
+		oltLogger.WithFields(log.Fields{
+			"IntfType": data.PktInd.IntfType,
+			"IntfId":   nniId,
+			"Pkt":      doubleTaggedPkt.Data(),
+		}).Tracef("Sent PktInd indication")
 	}
 }
 
-func (o OltDevice) FindOnu(serialNumber string) (*Onu, error) {
-
+// returns an ONU with a given Serial Number
+func (o OltDevice) FindOnuBySn(serialNumber string) (*Onu, error) {
+	// TODO this function can be a perfoormance bottlenec when we have many ONUs,
+	// memoizing it will remove the bottleneck
 	for _, pon := range o.Pons {
 		for _, onu := range pon.Onus {
 			if onu.Sn() == serialNumber {
@@ -390,7 +426,22 @@
 		}
 	}
 
-	return &Onu{}, errors.New(fmt.Sprintf("cannot-find-onu-%s", serialNumber))
+	return &Onu{}, errors.New(fmt.Sprintf("cannot-find-onu-by-serial-number-%s", serialNumber))
+}
+
+// returns an ONU with a given Mac Address
+func (o OltDevice) FindOnuByMacAddress(mac net.HardwareAddr) (*Onu, error) {
+	// TODO this function can be a perfoormance bottlenec when we have many ONUs,
+	// memoizing it will remove the bottleneck
+	for _, pon := range o.Pons {
+		for _, onu := range pon.Onus {
+			if onu.HwAddress.String() == mac.String() {
+				return &onu, nil
+			}
+		}
+	}
+
+	return &Onu{}, errors.New(fmt.Sprintf("cannot-find-onu-by-mac-address-%s", mac))
 }
 
 // GRPC Endpoints
diff --git a/internal/bbsim/devices/olt_test.go b/internal/bbsim/devices/olt_test.go
index a022c7c..f83fc62 100644
--- a/internal/bbsim/devices/olt_test.go
+++ b/internal/bbsim/devices/olt_test.go
@@ -18,6 +18,7 @@
 
 import (
 	"gotest.tools/assert"
+	"net"
 	"testing"
 )
 
@@ -32,10 +33,12 @@
 		}
 
 		for j := 0; j < numOnu; j++ {
+			onuId := uint32(i + j)
 			onu := Onu{
-				ID:        uint32(i + j),
+				ID:        onuId,
 				PonPort:   pon,
-				PonPortID: uint32(i),
+				PonPortID: pon.ID,
+				HwAddress: net.HardwareAddr{0x2e, 0x60, 0x70, 0x13, byte(pon.ID), byte(onuId)},
 			}
 			onu.SerialNumber = onu.NewSN(olt.ID, pon.ID, onu.ID)
 			pon.Onus = append(pon.Onus, onu)
@@ -45,14 +48,14 @@
 	return olt
 }
 
-func Test_Olt_FindOnu_Success(t *testing.T) {
+func Test_Olt_FindOnuBySn_Success(t *testing.T) {
 
 	numPon := 4
 	numOnu := 4
 
 	olt := createMockOlt(numPon, numOnu)
 
-	onu, err := olt.FindOnu("BBSM00000303")
+	onu, err := olt.FindOnuBySn("BBSM00000303")
 
 	assert.Equal(t, err, nil)
 	assert.Equal(t, onu.Sn(), "BBSM00000303")
@@ -60,14 +63,45 @@
 	assert.Equal(t, onu.PonPortID, uint32(3))
 }
 
-func Test_Olt_FindOnu_Error(t *testing.T) {
+func Test_Olt_FindOnuBySn_Error(t *testing.T) {
 
 	numPon := 1
 	numOnu := 4
 
 	olt := createMockOlt(numPon, numOnu)
 
-	_, err := olt.FindOnu("BBSM00000303")
+	_, err := olt.FindOnuBySn("BBSM00000303")
 
-	assert.Equal(t, err.Error(), "cannot-find-onu-BBSM00000303")
+	assert.Equal(t, err.Error(), "cannot-find-onu-by-serial-number-BBSM00000303")
+}
+
+func Test_Olt_FindOnuByMacAddress_Success(t *testing.T) {
+
+	numPon := 4
+	numOnu := 4
+
+	olt := createMockOlt(numPon, numOnu)
+
+	mac := net.HardwareAddr{0x2e, 0x60, 0x70, 0x13, byte(3), byte(3)}
+
+	onu, err := olt.FindOnuByMacAddress(mac)
+
+	assert.Equal(t, err, nil)
+	assert.Equal(t, onu.Sn(), "BBSM00000303")
+	assert.Equal(t, onu.ID, uint32(3))
+	assert.Equal(t, onu.PonPortID, uint32(3))
+}
+
+func Test_Olt_FindOnuByMacAddress_Error(t *testing.T) {
+
+	numPon := 1
+	numOnu := 4
+
+	olt := createMockOlt(numPon, numOnu)
+
+	mac := net.HardwareAddr{0x2e, 0x60, 0x70, 0x13, byte(3), byte(3)}
+
+	_, err := olt.FindOnuByMacAddress(mac)
+
+	assert.Equal(t, err.Error(), "cannot-find-onu-by-mac-address-2e:60:70:13:03:03")
 }
diff --git a/internal/bbsim/packetHandlers/filters.go b/internal/bbsim/packetHandlers/filters.go
index 3233c8c..0748839 100644
--- a/internal/bbsim/packetHandlers/filters.go
+++ b/internal/bbsim/packetHandlers/filters.go
@@ -17,6 +17,7 @@
 package packetHandlers
 
 import (
+	"errors"
 	"github.com/google/gopacket"
 	"github.com/google/gopacket/layers"
 	"net"
@@ -29,6 +30,9 @@
 	return false
 }
 
+// return true if the packet is coming in the OLT from the NNI port
+// it uses the ack to check if the source is the one we assigned to the
+// dhcp server
 func IsIncomingPacket(packet gopacket.Packet) bool {
 	if ipLayer := packet.Layer(layers.LayerTypeIPv4); ipLayer != nil {
 
@@ -41,3 +45,15 @@
 	}
 	return false
 }
+
+// returns the Desctination Mac Address contained in the packet
+func GetDstMacAddressFromPacket(packet gopacket.Packet) (net.HardwareAddr, error) {
+	if ethLayer := packet.Layer(layers.LayerTypeEthernet); ethLayer != nil {
+		eth, _ := ethLayer.(*layers.Ethernet)
+
+		if eth.DstMAC != nil {
+			return eth.DstMAC, nil
+		}
+	}
+	return nil, errors.New("cant-find-mac-address")
+}
diff --git a/internal/bbsim/packetHandlers/filters_test.go b/internal/bbsim/packetHandlers/filters_test.go
new file mode 100644
index 0000000..6d82495
--- /dev/null
+++ b/internal/bbsim/packetHandlers/filters_test.go
@@ -0,0 +1,122 @@
+/*
+ * Copyright 2018-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 packetHandlers_test
+
+import (
+	"github.com/google/gopacket"
+	"github.com/google/gopacket/layers"
+	"github.com/opencord/bbsim/internal/bbsim/packetHandlers"
+	"gotest.tools/assert"
+	"net"
+	"testing"
+)
+
+func Test_IsDhcpPacket_True(t *testing.T) {
+	dhcp := &layers.DHCPv4{
+		Operation: layers.DHCPOpReply,
+	}
+
+	buffer := gopacket.NewSerializeBuffer()
+	opts := gopacket.SerializeOptions{FixLengths: true}
+	err := gopacket.SerializeLayers(buffer, opts, dhcp)
+	if err != nil {
+		t.Fatal(err)
+	}
+
+	dhcpPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeDHCPv4, gopacket.DecodeOptions{})
+
+	res := packetHandlers.IsDhcpPacket(dhcpPkt)
+	assert.Equal(t, res, true)
+}
+
+func Test_IsDhcpPacket_False(t *testing.T) {
+	eth := &layers.Ethernet{
+		DstMAC: net.HardwareAddr{0x2e, 0x60, 0x70, 0x13, 0x15, 0x16},
+		SrcMAC: net.HardwareAddr{0x2e, 0x60, 0x70, 0x13, 0x15, 0x17},
+	}
+
+	buffer := gopacket.NewSerializeBuffer()
+	opts := gopacket.SerializeOptions{FixLengths: true}
+	err := gopacket.SerializeLayers(buffer, opts, eth)
+	if err != nil {
+		t.Fatal(err)
+	}
+
+	ethernetPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeEthernet, gopacket.DecodeOptions{})
+
+	res := packetHandlers.IsDhcpPacket(ethernetPkt)
+	assert.Equal(t, res, false)
+}
+
+func Test_IsIncomingPacket_True(t *testing.T) {
+	eth := &layers.IPv4{
+		SrcIP: net.ParseIP("182.21.0.128"),
+		DstIP: net.ParseIP("182.21.0.122"),
+	}
+
+	buffer := gopacket.NewSerializeBuffer()
+	opts := gopacket.SerializeOptions{FixLengths: true}
+	err := gopacket.SerializeLayers(buffer, opts, eth)
+	if err != nil {
+		t.Fatal(err)
+	}
+
+	ethernetPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeIPv4, gopacket.DecodeOptions{})
+
+	res := packetHandlers.IsIncomingPacket(ethernetPkt)
+	assert.Equal(t, res, true)
+}
+
+func Test_IsIncomingPacket_False(t *testing.T) {
+	eth := &layers.IPv4{
+		SrcIP: net.ParseIP("182.21.0.122"),
+		DstIP: net.ParseIP("182.21.0.128"),
+	}
+
+	buffer := gopacket.NewSerializeBuffer()
+	opts := gopacket.SerializeOptions{FixLengths: true}
+	err := gopacket.SerializeLayers(buffer, opts, eth)
+	if err != nil {
+		t.Fatal(err)
+	}
+
+	ethernetPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeIPv4, gopacket.DecodeOptions{})
+
+	res := packetHandlers.IsIncomingPacket(ethernetPkt)
+	assert.Equal(t, res, false)
+}
+
+func Test_GetDstMacAddressFromPacket(t *testing.T) {
+	dstMac := net.HardwareAddr{0x2e, 0x60, 0x70, 0x13, 0x15, 0x16}
+	eth := &layers.Ethernet{
+		DstMAC: dstMac,
+		SrcMAC: net.HardwareAddr{0x2e, 0x60, 0x70, 0x13, 0x15, 0x17},
+	}
+
+	buffer := gopacket.NewSerializeBuffer()
+	opts := gopacket.SerializeOptions{FixLengths: true}
+	err := gopacket.SerializeLayers(buffer, opts, eth)
+	if err != nil {
+		t.Fatal(err)
+	}
+
+	ethernetPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeEthernet, gopacket.DecodeOptions{})
+
+	res, err := packetHandlers.GetDstMacAddressFromPacket(ethernetPkt)
+	assert.Equal(t, err, nil)
+	assert.Equal(t, res.String(), dstMac.String())
+}
diff --git a/internal/bbsim/packetHandlers/packet_tags.go b/internal/bbsim/packetHandlers/packet_tags.go
index 3e31e3c..ebcb96c 100644
--- a/internal/bbsim/packetHandlers/packet_tags.go
+++ b/internal/bbsim/packetHandlers/packet_tags.go
@@ -57,6 +57,20 @@
 	return nil, errors.New("Couldn't extract LayerTypeEthernet from packet")
 }
 
+func PushDoubleTag(stag int, ctag int, pkt gopacket.Packet) (gopacket.Packet, error) {
+
+	singleTaggedPkt, err := PushSingleTag(ctag, pkt)
+	if err != nil {
+		return nil, err
+	}
+	doubleTaggedPkt, err := PushSingleTag(stag, singleTaggedPkt)
+	if err != nil {
+		return nil, err
+	}
+
+	return doubleTaggedPkt, nil
+}
+
 func PopSingleTag(pkt gopacket.Packet) (gopacket.Packet, error) {
 	layer, err := getDot1QLayer(pkt)
 	if err != nil {
@@ -114,7 +128,7 @@
 	return nil, errors.New("no-dot1q-layer-in-packet")
 }
 
-func getVlanTag(pkt gopacket.Packet) (uint16, error) {
+func GetVlanTag(pkt gopacket.Packet) (uint16, error) {
 	dot1q, err := getDot1QLayer(pkt)
 	if err != nil {
 		return 0, err
diff --git a/internal/bbsim/packetHandlers/packet_tags_test.go b/internal/bbsim/packetHandlers/packet_tags_test.go
index 91c944e..9ab5112 100644
--- a/internal/bbsim/packetHandlers/packet_tags_test.go
+++ b/internal/bbsim/packetHandlers/packet_tags_test.go
@@ -14,34 +14,17 @@
  * limitations under the License.
  */
 
-package packetHandlers
+package packetHandlers_test
 
 import (
-	"fmt"
 	"github.com/google/gopacket"
 	"github.com/google/gopacket/layers"
+	"github.com/opencord/bbsim/internal/bbsim/packetHandlers"
 	"gotest.tools/assert"
 	"net"
-	"os"
 	"testing"
 )
 
-func setUp() {
-	fmt.Println("Test Setup")
-}
-
-func tearDown() {
-	fmt.Println("Test Teardown")
-}
-
-func TestMain(m *testing.M) {
-	setUp()
-	code := m.Run()
-	tearDown()
-	os.Exit(code)
-}
-
-// GO111MODULE=on go test -v -mod vendor ./internal/bbsim/... -run TestPushSingleTag
 func TestPushSingleTag(t *testing.T) {
 	rawBytes := []byte{10, 20, 30}
 	srcMac := net.HardwareAddr{0xff, 0xff, 0xff, 0xff, byte(1), byte(1)}
@@ -64,16 +47,61 @@
 	)
 
 	untaggedPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeEthernet, gopacket.Default)
-	taggedPkt, err := PushSingleTag(111, untaggedPkt)
+	taggedPkt, err := packetHandlers.PushSingleTag(111, untaggedPkt)
 	if err != nil {
 		t.Fail()
 		t.Logf("Error in PushSingleTag: %v", err)
 	}
 
-	vlan, _ := getVlanTag(taggedPkt)
+	vlan, _ := packetHandlers.GetVlanTag(taggedPkt)
 	assert.Equal(t, vlan, uint16(111))
 }
 
+func TestPushDoubleTag(t *testing.T) {
+	rawBytes := []byte{10, 20, 30}
+	srcMac := net.HardwareAddr{0xff, 0xff, 0xff, 0xff, byte(1), byte(1)}
+	dstMac := net.HardwareAddr{0xff, 0xff, 0xff, 0xff, 0xff, 0xff}
+
+	ethernetLayer := &layers.Ethernet{
+		SrcMAC:       srcMac,
+		DstMAC:       dstMac,
+		EthernetType: 0x8100,
+	}
+
+	buffer := gopacket.NewSerializeBuffer()
+	gopacket.SerializeLayers(
+		buffer,
+		gopacket.SerializeOptions{
+			FixLengths: false,
+		},
+		ethernetLayer,
+		gopacket.Payload(rawBytes),
+	)
+
+	untaggedPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeEthernet, gopacket.Default)
+	taggedPkt, err := packetHandlers.PushDoubleTag(900, 800, untaggedPkt)
+	if err != nil {
+		t.Fail()
+		t.Logf("Error in PushSingleTag: %v", err)
+	}
+
+	sTag, err := packetHandlers.GetVlanTag(taggedPkt)
+	if err != nil {
+		t.Fatalf(err.Error())
+	}
+	singleTagPkt, err := packetHandlers.PopSingleTag(taggedPkt)
+	if err != nil {
+		t.Fatalf(err.Error())
+	}
+	cTag, err := packetHandlers.GetVlanTag(singleTagPkt)
+	if err != nil {
+		t.Fatalf(err.Error())
+	}
+
+	assert.Equal(t, sTag, uint16(900))
+	assert.Equal(t, cTag, uint16(800))
+}
+
 func TestPopSingleTag(t *testing.T) {
 	rawBytes := []byte{10, 20, 30}
 	srcMac := net.HardwareAddr{0xff, 0xff, 0xff, 0xff, byte(1), byte(1)}
@@ -102,13 +130,13 @@
 	)
 
 	untaggedPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeEthernet, gopacket.Default)
-	taggedPkt, err := PopSingleTag(untaggedPkt)
+	taggedPkt, err := packetHandlers.PopSingleTag(untaggedPkt)
 	if err != nil {
 		t.Fail()
 		t.Logf("Error in PushSingleTag: %v", err)
 	}
 
-	vlan, err := getVlanTag(taggedPkt)
+	vlan, err := packetHandlers.GetVlanTag(taggedPkt)
 	assert.Equal(t, vlan, uint16(2580)) // FIXME where dows 2056 comes from??
 }
 
@@ -140,13 +168,13 @@
 	)
 
 	untaggedPkt := gopacket.NewPacket(buffer.Bytes(), layers.LayerTypeEthernet, gopacket.Default)
-	taggedPkt, err := PopDoubleTag(untaggedPkt)
+	taggedPkt, err := packetHandlers.PopDoubleTag(untaggedPkt)
 	if err != nil {
 		t.Fail()
 		t.Logf("Error in PushSingleTag: %v", err)
 	}
 
-	vlan, err := getVlanTag(taggedPkt)
+	vlan, err := packetHandlers.GetVlanTag(taggedPkt)
 	assert.Equal(t, vlan, uint16(0))
 	assert.Equal(t, err.Error(), "no-dot1q-layer-in-packet")
 }