diff --git a/core/core_server.go b/core/core_server.go
index be6b04a..8b52e49 100644
--- a/core/core_server.go
+++ b/core/core_server.go
@@ -17,7 +17,6 @@
 package core
 
 import (
-	"errors"
 	"gerrit.opencord.org/voltha-bbsim/device"
 	"gerrit.opencord.org/voltha-bbsim/protos"
 	"gerrit.opencord.org/voltha-bbsim/setup"
@@ -25,6 +24,7 @@
 	"github.com/google/gopacket/layers"
 	"github.com/google/gopacket/pcap"
 	"google.golang.org/grpc"
+	"errors"
 	"log"
 	"strconv"
 	"sync"
@@ -55,6 +55,7 @@
 	TestFlag     bool
 	Processes    []string
 	EnableServer *openolt.Openolt_EnableIndicationServer
+	CtagMap      map[string]uint32
 }
 
 type Packet struct {
@@ -88,6 +89,15 @@
 		s.Onumap[intfid] = device.CreateOnus(oltid, intfid, nonus, nnni)
 	}
 	s.EnableServer = new(openolt.Openolt_EnableIndicationServer)
+
+	//TODO: To be fixed
+	s.CtagMap = make(map[string]uint32)
+	for i := 0; i < MAX_ONUS_PER_PON; i ++ {
+		oltid := s.Olt.ID
+		intfid := uint32(1)
+		sn := convB2S(device.CreateSN(oltid, intfid, uint32(i)))
+		s.CtagMap[sn] = uint32(900 + i)	// This is hard coded for BBWF
+	}
 	return s
 }
 
@@ -132,7 +142,7 @@
 
 	// OLT Sends OnuInd after waiting all of those ONUs up
 	for {
-		if s.IsAllONUActive() {
+		if s.IsAllOnuActive(s.Onumap) {
 			break
 		}
 	}
@@ -235,7 +245,7 @@
 	if handler, vethenv, err = setupVethHandler(nniup, nnidw, vethenv); err != nil {
 		return ioinfos, vethenv, err
 	}
-	//TODO: Intfid should be modified
+
 	iinfo := Ioinfo{name: nnidw, iotype: "nni", ioloc: "inside", intfid: 1, handler: handler}
 	ioinfos = append(ioinfos, &iinfo)
 	oinfo := Ioinfo{name: nniup, iotype: "nni", ioloc: "outside", intfid: 1, handler: nil}
@@ -288,14 +298,27 @@
 			layerEth := pkt.Layer(layers.LayerTypeEthernet)
 			le, _ := layerEth.(*layers.Ethernet)
 			ethtype := le.EthernetType
+
 			if ethtype == 0x888e {
 				log.Printf("Received upstream packet is EAPOL.")
 				//log.Println(unipkt.Pkt.Dump())
 				//log.Println(pkt.Dump())
 			} else if layerDHCP := pkt.Layer(layers.LayerTypeDHCPv4); layerDHCP != nil {
 				log.Printf("Received upstream packet is DHCP.")
-				//log.Println(unipkt.Pkt.Dump())
-				//log.Println(pkt.Dump())
+
+				//C-TAG
+				onu, _ := s.getOnuByID(onuid)
+				sn := convB2S(onu.SerialNumber.VendorSpecific)
+				if ctag, ok := s.CtagMap[sn]; ok == true{
+					tagpkt, err := PushVLAN(pkt, uint16(ctag))
+					if err != nil {
+						log.Println("Error happend in C-tag tagging")
+					} else {
+						pkt = tagpkt
+					}
+				} else {
+					log.Printf("Could not find the onuid %d (SN: %s) in CtagMap %v!\n", onuid, sn, s.CtagMap)
+				}
 			} else {
 				continue
 			}
@@ -441,7 +464,6 @@
 }
 
 func (s *Server) uplinkPacketOut(rawpkt gopacket.Packet) error {
-	log.Println("")
 	poppkt, _, err := PopVLAN(rawpkt)
 	poppkt, _, err = PopVLAN(poppkt)
 	if err != nil {
@@ -457,10 +479,10 @@
 	return nil
 }
 
-func (s *Server) IsAllONUActive() bool {
-	for _, onus := range s.Onumap {
+func (s *Server)IsAllOnuActive(regonus map[uint32][]*device.Onu) bool {
+	for _, onus := range regonus {
 		for _, onu := range onus {
-			if *onu.InternalState != device.ONU_ACTIVATED {
+			if onu.GetIntStatus() != device.ONU_ACTIVATED {
 				return false
 			}
 		}
@@ -558,3 +580,11 @@
 	log.Printf("Server handle created for %s\n", vethname)
 	return handle, nil
 }
+
+func convB2S(b []byte) string {
+	s := ""
+	for _, i := range b {
+		s = s + strconv. FormatInt(int64(i/16), 16) + strconv. FormatInt(int64(i%16), 16)
+	}
+	return s
+}
\ No newline at end of file
diff --git a/core/grpc_service.go b/core/grpc_service.go
index 90bdf35..329292d 100644
--- a/core/grpc_service.go
+++ b/core/grpc_service.go
@@ -31,10 +31,12 @@
 // gRPC Service
 func (s *Server) DisableOlt(c context.Context, empty *openolt.Empty) (*openolt.Empty, error) {
 	log.Printf("OLT receives DisableOLT()\n")
-	if err := sendOltIndDown(*s.EnableServer); err != nil {
-		return new(openolt.Empty), err
+	if s.EnableServer != nil {
+		if err := sendOltIndDown(*s.EnableServer); err != nil {
+			return new(openolt.Empty), err
+		}
+		log.Println("Successfuly sent OLT DOWN indication !")
 	}
-	log.Println("Successfuly sent OLT DOWN indication !")
 	return new(openolt.Empty), nil
 }
 
@@ -68,7 +70,7 @@
 		}
 		onuid := onu.OnuId
 		matched.OnuID = onuid
-		*matched.InternalState = device.ONU_ACTIVATED
+		matched.UpdateIntStatus(device.ONU_ACTIVATED)
 		log.Printf("ONU IntfID: %d OnuID: %d activated succesufully.\n", onu.IntfId, onu.OnuId)
 	}
 	return new(openolt.Empty), nil
@@ -112,6 +114,10 @@
 
 func (s *Server) FlowAdd(c context.Context, flow *openolt.Flow) (*openolt.Empty, error) {
 	log.Printf("OLT %d receives FlowAdd().\n", s.Olt.ID)
+	log.Printf("Flow's ONU-ID: %d, CTAG: %d\n", flow.OnuId, flow.Action.IVid)
+	//onuid := uint32(flow.OnuId)
+	//ctag := flow.Action.IVid
+	//s.CtagMap[onuid] = ctag
 	return new(openolt.Empty), nil
 }
 
diff --git a/core/io_info.go b/core/io_info.go
index 4252509..c913192 100644
--- a/core/io_info.go
+++ b/core/io_info.go
@@ -17,8 +17,8 @@
 package core
 
 import (
-	"errors"
 	"github.com/google/gopacket/pcap"
+	"errors"
 	"log"
 )
 
diff --git a/core/io_worker.go b/core/io_worker.go
index d512b01..3b59b49 100644
--- a/core/io_worker.go
+++ b/core/io_worker.go
@@ -17,10 +17,10 @@
 package core
 
 import (
-	"errors"
 	"github.com/google/gopacket"
 	"github.com/google/gopacket/layers"
 	"github.com/google/gopacket/pcap"
+	"errors"
 	"log"
 	"net"
 )
@@ -39,13 +39,19 @@
 }
 
 func SendUni(handle *pcap.Handle, packet gopacket.Packet) {
-	handle.WritePacketData(packet.Data())
-	log.Printf("send packet to UNI-IF: %v \n", *handle)
+	err := handle.WritePacketData(packet.Data())
+	if err != nil {
+		log.Printf("Error in send packet to UNI-IF: %v e:%s\n", *handle, err)
+	}
+	log.Printf("Successfully send packet to UNI-IF: %v \n", *handle)
 	//log.Println(packet.Dump())
 }
 
 func SendNni(handle *pcap.Handle, packet gopacket.Packet) {
-	handle.WritePacketData(packet.Data())
+	err := handle.WritePacketData(packet.Data())
+	if err != nil{
+		log.Printf("Error in send packet to NNI e:%s\n", err)
+	}
 	log.Printf("send packet to NNI-IF: %v \n", *handle)
 	//log.Println(packet.Dump())
 }
diff --git a/device/device_onu.go b/device/device_onu.go
index 65499a8..c0b9ff9 100644
--- a/device/device_onu.go
+++ b/device/device_onu.go
@@ -17,10 +17,10 @@
 package device
 
 import (
+ 	"gerrit.opencord.org/voltha-bbsim/protos"
 	"log"
 	"reflect"
-
-	"gerrit.opencord.org/voltha-bbsim/protos"
+	"sync"
 )
 
 type onuState int
@@ -36,9 +36,10 @@
 	OperState     string
 	SerialNumber  *openolt.SerialNumber
 	OnuID         uint32
+	mu            *sync.Mutex
 }
 
-func createSN(oltid uint32, intfid uint32, onuid uint32) []byte {
+func CreateSN(oltid uint32, intfid uint32, onuid uint32) []byte {
 	sn := []byte{0, byte(oltid % 256), byte(intfid), byte(onuid)}
 	return sn
 }
@@ -49,11 +50,12 @@
 		onu := Onu{}
 		onu.InternalState = new(onuState)
 		*onu.InternalState = ONU_PRE_ACTIVATED
+		onu.mu = &sync.Mutex{}
 		onu.IntfID = intfid
 		onu.OperState = "up"
 		onu.SerialNumber = new(openolt.SerialNumber)
 		onu.SerialNumber.VendorId = []byte("BBSM")
-		onu.SerialNumber.VendorSpecific = createSN(oltid, intfid, uint32(i))
+		onu.SerialNumber.VendorSpecific = CreateSN(oltid, intfid, uint32(i))
 		onus = append(onus, &onu)
 	}
 	return onus
@@ -85,3 +87,15 @@
 		log.Printf("(PONIF:%d) ONU [%d] discovered.\n", ponif, i)
 	}
 }
+
+func (onu *Onu) UpdateIntStatus(intstatus onuState){
+	onu.mu.Lock()
+	defer onu.mu.Unlock()
+	*onu.InternalState = intstatus
+}
+
+func (onu *Onu) GetIntStatus() onuState{
+	onu.mu.Lock()
+	defer onu.mu.Unlock()
+	return *onu.InternalState
+}