Merge "More logs"
diff --git a/common/logger/logger.go b/common/logger/logger.go
index 5830e3b..444cabb 100644
--- a/common/logger/logger.go
+++ b/common/logger/logger.go
@@ -19,6 +19,7 @@
 import (
 	lkh "github.com/gfremex/logrus-kafka-hook"
 	log "github.com/sirupsen/logrus"
+	"time"
 )
 
 var (
@@ -42,9 +43,10 @@
 			"kh",
 			[]log.Level{log.DebugLevel, log.InfoLevel, log.WarnLevel, log.ErrorLevel},
 			&log.JSONFormatter{
+				TimestampFormat: time.RFC3339Nano,
 				FieldMap: log.FieldMap{
 					log.FieldKeyTime:  "@timestamp",
-					log.FieldKeyLevel: "level",
+					log.FieldKeyLevel: "levelname",
 					log.FieldKeyMsg:   "message",
 				},
 			},
diff --git a/core/core_server.go b/core/core_server.go
index 3e480cb..69d6d69 100644
--- a/core/core_server.go
+++ b/core/core_server.go
@@ -57,8 +57,8 @@
 	cancel       context.CancelFunc
 	state        coreState
 	stateChan    chan coreState
-	omciIn       chan OmciMsg
-	omciOut      chan OmciMsg
+	omciIn       chan openolt.OmciIndication
+	omciOut      chan openolt.OmciMsg
 }
 
 type Packet struct {
@@ -80,7 +80,7 @@
        <-              <-
 */
 
-func NewCore(opt *option, omciOut chan OmciMsg, omciIn chan OmciMsg) *Server {
+func NewCore(opt *option) *Server {
 	// TODO: make it decent
 	oltid := opt.oltid
 	npon := opt.npon
@@ -97,8 +97,8 @@
 		EnableServer: nil,
 		state:        INACTIVE,
 		stateChan:    make(chan coreState, 8),
-		omciIn:       omciIn,
-		omciOut:      omciOut,
+		omciIn:       make(chan openolt.OmciIndication, 1024),
+		omciOut:      make(chan openolt.OmciMsg, 1024),
 	}
 
 	nnni := s.Olt.NumNniIntf
@@ -330,6 +330,16 @@
 	defer logger.Debug("runPacketInDaemon Done")
 	unichannel := make(chan Packet, 2048)
 
+
+	logger.Debug("runOMCIDaemon Start")
+	defer logger.Debug("runOMCIDaemon Done")
+	errch := make (chan error)
+	OmciRun(s.omciOut, s.omciIn, s.Onumap, errch)
+	go func(){
+		<-errch	// Wait for OmciInitialization
+		s.updateState(ACTIVE)
+	}()
+
 	for intfid, _ := range s.Onumap {
 		for _, onu := range s.Onumap[intfid] {
 			onuid := onu.OnuID
@@ -352,12 +362,11 @@
 	go RecvWorker(ioinfo, nhandler, nnichannel)
 
 	data := &openolt.Indication_PktInd{}
-	s.updateState(ACTIVE)
 	for {
 		select {
 		case msg := <-s.omciIn:
 			logger.Debug("OLT %d send omci indication, IF %v (ONU-ID: %v) pkt:%x.", s.Olt.ID, msg.IntfId, msg.OnuId, msg.Pkt)
-			omci := &openolt.Indication_OmciInd{OmciInd: &openolt.OmciIndication{IntfId: msg.IntfId, OnuId: msg.OnuId, Pkt: msg.Pkt}}
+			omci := &openolt.Indication_OmciInd{OmciInd: &msg}
 			if err := stream.Send(&openolt.Indication{Data: omci}); err != nil {
 				logger.Error("send omci indication failed.", err)
 				continue
@@ -508,9 +517,15 @@
 }
 
 func getGemPortID(intfid uint32, onuid uint32) (uint32, error) {
-	idx := uint32(0)
-	return 1024 + (((MAX_ONUS_PER_PON*intfid + onuid - 1) * 7) + idx), nil
-	//return uint32(1032 + 8 * (vid - 1)), nil
+	key := OnuKey{intfid, onuid}
+	if onuState, ok := OnuOmciStateMap[key]; !ok {
+		idx := uint32(0)
+		// Backwards compatible with bbsim_olt adapter
+		return 1024 + (((MAX_ONUS_PER_PON*intfid + onuid - 1) * 7) + idx), nil
+	} else {
+		// FIXME - Gem Port ID is 2 bytes - fix openolt.proto
+		return uint32(onuState.gemPortId), nil
+	}
 }
 
 func getOnuBySN(onumap map[uint32][]*device.Onu, sn *openolt.SerialNumber) (*device.Onu, error) {
diff --git a/core/grpc_service.go b/core/grpc_service.go
index 8678d87..2175a75 100644
--- a/core/grpc_service.go
+++ b/core/grpc_service.go
@@ -100,12 +100,8 @@
 }
 
 func (s *Server) OmciMsgOut(c context.Context, msg *openolt.OmciMsg) (*openolt.Empty, error) {
-	var resp OmciMsg
 	logger.Debug("OLT %d receives OmciMsgOut to IF %v (ONU-ID: %v) pkt:%x.", s.Olt.ID, msg.IntfId, msg.OnuId, msg.Pkt)
-	resp.IntfId = msg.IntfId
-	resp.OnuId = msg.OnuId
-	resp.Pkt = make([]byte, len(msg.Pkt))
-	s.omciOut <- resp
+	s.omciOut <- *msg
 	return new(openolt.Empty), nil
 }
 
diff --git a/core/mediator.go b/core/mediator.go
index 0ad0696..ee25264 100644
--- a/core/mediator.go
+++ b/core/mediator.go
@@ -125,10 +125,7 @@
 func (m *mediator) Start() {
 	var wg sync.WaitGroup
 	opt := m.opt
-	omciOut := make(chan OmciMsg, 8)
-	omciIn := make(chan OmciMsg, 8)
-	go OmciRun(omciOut, omciIn)
-	server := NewCore(opt, omciOut, omciIn)
+	server := NewCore(opt)
 	wg.Add(1)
 	go func() {
 		if err := server.Start(); err != nil { //Blocking
diff --git a/core/omci.go b/core/omci.go
index f003712..acf7bbe 100644
--- a/core/omci.go
+++ b/core/omci.go
@@ -16,18 +16,352 @@
 
 package core
 
-import "gerrit.opencord.org/voltha-bbsim/common/logger"
+import (
+	"bytes"
+	"encoding/binary"
 
-type OmciMsg struct {
-	IntfId uint32
-	OnuId  uint32
-	Pkt    []byte
+	"gerrit.opencord.org/voltha-bbsim/common/logger"
+	"gerrit.opencord.org/voltha-bbsim/protos"
+	"gerrit.opencord.org/voltha-bbsim/device"
+	"time"
+)
+
+//
+// OMCI definitions
+//
+
+// OmciMsgType represents a OMCI message-type
+type OmciMsgType byte
+
+const (
+	// Message Types
+	_                                 = iota
+	Create                OmciMsgType = 4
+	Delete                OmciMsgType = 6
+	Set                   OmciMsgType = 8
+	Get                   OmciMsgType = 9
+	GetAllAlarms          OmciMsgType = 11
+	GetAllAlarmsNext      OmciMsgType = 12
+	MibUpload             OmciMsgType = 13
+	MibUploadNext         OmciMsgType = 14
+	MibReset              OmciMsgType = 15
+	AlarmNotification     OmciMsgType = 16
+	AttributeValueChange  OmciMsgType = 17
+	Test                  OmciMsgType = 18
+	StartSoftwareDownload OmciMsgType = 19
+	DownloadSection       OmciMsgType = 20
+	EndSoftwareDownload   OmciMsgType = 21
+	ActivateSoftware      OmciMsgType = 22
+	CommitSoftware        OmciMsgType = 23
+	SynchronizeTime       OmciMsgType = 24
+	Reboot                OmciMsgType = 25
+	GetNext               OmciMsgType = 26
+	TestResult            OmciMsgType = 27
+	GetCurrentData        OmciMsgType = 28
+	SetTable              OmciMsgType = 29 // Defined in Extended Message Set Only
+)
+
+const (
+	// Managed Entity Class values
+	GEMPortNetworkCTP OmciClass = 268
+)
+
+// OMCI Managed Entity Class
+type OmciClass uint16
+
+// OMCI Message Identifier
+type OmciMessageIdentifier struct {
+	Class    OmciClass
+	Instance uint16
 }
 
-func OmciRun(omciOut chan OmciMsg, omciIn chan OmciMsg) {
-	for {
-		msg := <-omciOut
-		logger.Debug("OmciRun IF %v (ONU-ID: %v) pkt:%x.", msg.IntfId, msg.OnuId, msg.Pkt)
-		omciIn <- msg
+type OmciContent [32]byte
+
+type OmciMessage struct {
+	TransactionId uint16
+	MessageType   OmciMsgType
+	DeviceId      uint8
+	MessageId     OmciMessageIdentifier
+	Content       OmciContent
+}
+
+const NumMibUploads byte = 9
+
+type OnuKey struct {
+	IntfId, OnuId uint32
+}
+
+type OnuOmciState struct {
+	gemPortId    uint16
+	mibUploadCtr uint16
+	uniGInstance uint8
+	pptpInstance uint8
+	init  istate
+}
+
+type istate int
+
+const (
+	INCOMPLETE istate = iota
+	DONE
+)
+
+type OmciMsgHandler func(class OmciClass, content OmciContent, key OnuKey) []byte
+
+var Handlers = map[OmciMsgType]OmciMsgHandler{
+	MibReset:      mibReset,
+	MibUpload:     mibUpload,
+	MibUploadNext: mibUploadNext,
+	Set:           set,
+	Create:        create,
+	Get:           get,
+	GetAllAlarms:  getAllAlarms,
+}
+
+var OnuOmciStateMap = map[OnuKey]*OnuOmciState{}
+
+func OmciRun(omciOut chan openolt.OmciMsg, omciIn chan openolt.OmciIndication, onumap map[uint32][] *device.Onu, errch chan error) {
+	go func() { //For monitoring the OMCI states
+		for {
+			time.Sleep(1 * time.Second)
+			if isAllOmciInitDone(onumap) {
+				logger.Info("OmciRun - All the omci init process were done")
+				close(errch)
+				break
+			}
+		}
+	}()
+
+	go func(){
+		for {
+			var resp openolt.OmciIndication
+
+			m := <-omciOut
+
+			transactionId, deviceId, msgType, class, instance, content := ParsePkt(m.Pkt)
+
+			logger.Debug("OmciRun - transactionId: %d msgType: %d, ME Class: %d, ME Instance: %d",
+				transactionId, msgType, class, instance)
+
+			key := OnuKey{m.IntfId, m.OnuId}
+			if _, ok := OnuOmciStateMap[key]; !ok {
+				OnuOmciStateMap[key] = NewOnuOmciState()
+			}
+
+			if _, ok := Handlers[msgType]; !ok {
+				logger.Warn("Ignore omci msg (msgType %d not handled)", msgType)
+				continue
+			}
+
+			resp.Pkt = Handlers[msgType](class, content, key)
+
+			resp.Pkt[0] = byte(transactionId >> 8)
+			resp.Pkt[1] = byte(transactionId & 0xFF)
+			resp.Pkt[2] = 0x2<<4 | byte(msgType)
+			resp.Pkt[3] = deviceId
+			resp.IntfId = m.IntfId
+			resp.OnuId = m.OnuId
+			omciIn <- resp
+		}
+	}()
+}
+
+func ParsePkt(pkt []byte) (uint16, uint8, OmciMsgType, OmciClass, uint16, OmciContent) {
+	var m OmciMessage
+
+	r := bytes.NewReader(HexDecode(pkt))
+
+	if err := binary.Read(r, binary.BigEndian, &m); err != nil {
+		logger.Error("binary.Read failed: %s", err)
 	}
+	logger.Debug("OmciRun - TransactionId: %d MessageType: %d, ME Class: %d, ME Instance: %d, Content: %x",
+		m.TransactionId, m.MessageType&0x0F, m.MessageId.Class, m.MessageId.Instance, m.Content)
+	return m.TransactionId, m.DeviceId, m.MessageType & 0x0F, m.MessageId.Class, m.MessageId.Instance, m.Content
+
 }
+
+func HexDecode(pkt []byte) []byte {
+	// Convert the hex encoding to binary
+	// TODO - Change openolt adapter to send raw binary instead of hex encoded
+	p := make([]byte, len(pkt)/2)
+	for i, j := 0, 0; i < len(pkt); i, j = i+2, j+1 {
+		// Go figure this ;)
+		u := (pkt[i] & 15) + (pkt[i]>>6)*9
+		l := (pkt[i+1] & 15) + (pkt[i+1]>>6)*9
+		p[j] = u<<4 + l
+	}
+	logger.Debug("Omci decoded: %x.", p)
+	return p
+}
+
+func NewOnuOmciState() *OnuOmciState {
+	return &OnuOmciState{gemPortId: 0, mibUploadCtr: 0, uniGInstance: 1, pptpInstance: 1}
+}
+
+func mibReset(class OmciClass, content OmciContent, key OnuKey) []byte {
+	var pkt []byte
+
+	logger.Debug("Omci MibReset")
+
+	pkt = []byte{
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+	return pkt
+}
+
+func mibUpload(class OmciClass, content OmciContent, key OnuKey) []byte {
+	var pkt []byte
+
+	logger.Debug("Omci MibUpload")
+
+	pkt = []byte{
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+
+	pkt[9] = NumMibUploads // Number of subsequent MibUploadNext cmds
+
+	return pkt
+}
+
+func mibUploadNext(class OmciClass, content OmciContent, key OnuKey) []byte {
+	var pkt []byte
+
+	logger.Debug("Omci MibUploadNext")
+
+	state := OnuOmciStateMap[key]
+
+	switch state.mibUploadCtr {
+	case 0:
+		// ANI-G
+		pkt = []byte{
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x01, 0x07, 0x80, 0x01, 0xff, 0xff, 0x01, 0x00,
+			0x08, 0x00, 0x30, 0x00, 0x00, 0x05, 0x09, 0x00,
+			0x00, 0xe0, 0x54, 0xff, 0xff, 0x00, 0x00, 0x0c,
+			0x63, 0x81, 0x81, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+	case 1, 2, 3, 4:
+		// UNI-G
+		pkt = []byte{
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x01, 0x08, 0x01, 0x01, 0xf8, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+		pkt[11] = state.uniGInstance // ME Instance
+		state.uniGInstance++
+	case 5, 6, 7, 8:
+		pkt = []byte{
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x00, 0x0b, 0x01, 0x01, 0xff, 0xfe, 0x00, 0x2f,
+			0x00, 0x00, 0x00, 0x00, 0x03, 0x05, 0xee, 0x00,
+			0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+		pkt[11] = state.pptpInstance // ME Instance
+		state.pptpInstance++
+	default:
+		logger.Error("Invalid MibUpload request %d", state.mibUploadCtr)
+	}
+
+	state.mibUploadCtr++
+
+	return pkt
+}
+
+func set(class OmciClass, content OmciContent, key OnuKey) []byte {
+	var pkt []byte
+
+	pkt = []byte{
+		0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+
+	logger.Debug("Omci Set")
+
+	return pkt
+}
+
+func create(class OmciClass, content OmciContent, key OnuKey) []byte {
+	var pkt []byte
+
+	if class == GEMPortNetworkCTP {
+		if onuOmciState, ok := OnuOmciStateMap[key]; !ok {
+			logger.Error("ONU Key Error - IntfId: %d, OnuId:", key.IntfId, key.OnuId)
+		} else {
+			onuOmciState.gemPortId = binary.BigEndian.Uint16(content[:2])
+			logger.Debug("Gem Port Id %d", onuOmciState.gemPortId)
+			OnuOmciStateMap[key].init = DONE
+		}
+	}
+
+	pkt = []byte{
+		0x00, 0x00, 0x00, 0x00, 0x01, 0x10, 0x00, 0x01,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+
+	logger.Debug("Omci Create")
+
+	return pkt
+}
+
+func get(class OmciClass, content OmciContent, key OnuKey) []byte {
+	var pkt []byte
+
+	pkt = []byte{
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x2d, 0x02, 0x01,
+		0x00, 0x20, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+
+	logger.Debug("Omci Get")
+
+	return pkt
+}
+
+func getAllAlarms(class OmciClass, content OmciContent, key OnuKey) []byte {
+	var pkt []byte
+
+	pkt = []byte{
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+		0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+
+	logger.Debug("Omci GetAllAlarms")
+
+	return pkt
+}
+
+func isAllOmciInitDone(onumap map[uint32][] *device.Onu) bool {
+	for _, onus := range onumap {
+		for _, onu := range onus{
+			key := OnuKey{onu.IntfID, onu.OnuID}
+			state := OnuOmciStateMap[key]
+			if state.init == INCOMPLETE{
+				return false
+			}
+		}
+	}
+	return true
+}
+