diff --git a/internal/pkg/onuadaptercore/device_handler.go b/internal/pkg/onuadaptercore/device_handler.go
index 54e778b..67fccc7 100644
--- a/internal/pkg/onuadaptercore/device_handler.go
+++ b/internal/pkg/onuadaptercore/device_handler.go
@@ -185,6 +185,7 @@
 	pOnuTP            *onuUniTechProf
 	pOnuMetricsMgr    *onuMetricsManager
 	pAlarmMgr         *onuAlarmManager
+	pSelfTestHdlr     *selfTestControlBlock
 	exitChannel       chan int
 	lockDevice        sync.RWMutex
 	pOnuIndication    *oop.OnuIndication
@@ -1369,13 +1370,14 @@
 
 //setOnuDeviceEntry sets the ONU device entry within the handler
 func (dh *deviceHandler) setOnuDeviceEntry(
-	apDeviceEntry *OnuDeviceEntry, apOnuTp *onuUniTechProf, apOnuMetricsMgr *onuMetricsManager, apOnuAlarmMgr *onuAlarmManager) {
+	apDeviceEntry *OnuDeviceEntry, apOnuTp *onuUniTechProf, apOnuMetricsMgr *onuMetricsManager, apOnuAlarmMgr *onuAlarmManager, apSelfTestHdlr *selfTestControlBlock) {
 	dh.lockDevice.Lock()
 	defer dh.lockDevice.Unlock()
 	dh.pOnuOmciDevice = apDeviceEntry
 	dh.pOnuTP = apOnuTp
 	dh.pOnuMetricsMgr = apOnuMetricsMgr
 	dh.pAlarmMgr = apOnuAlarmMgr
+	dh.pSelfTestHdlr = apSelfTestHdlr
 }
 
 //addOnuDeviceEntry creates a new ONU device or returns the existing
@@ -1392,8 +1394,9 @@
 		onuTechProfProc := newOnuUniTechProf(ctx, dh)
 		onuMetricsMgr := newonuMetricsManager(ctx, dh)
 		onuAlarmManager := newAlarmManager(ctx, dh)
+		selfTestHdlr := newSelfTestMsgHandlerCb(ctx, dh)
 		//error treatment possible //TODO!!!
-		dh.setOnuDeviceEntry(deviceEntry, onuTechProfProc, onuMetricsMgr, onuAlarmManager)
+		dh.setOnuDeviceEntry(deviceEntry, onuTechProfProc, onuMetricsMgr, onuAlarmManager, selfTestHdlr)
 		// fire deviceEntry ready event to spread to possibly waiting processing
 		dh.deviceEntrySet <- true
 		logger.Debugw(ctx, "onuDeviceEntry-added", log.Fields{"device-id": dh.deviceID})
diff --git a/internal/pkg/onuadaptercore/omci_cc.go b/internal/pkg/onuadaptercore/omci_cc.go
index 7d28169..b37230b 100644
--- a/internal/pkg/onuadaptercore/omci_cc.go
+++ b/internal/pkg/onuadaptercore/omci_cc.go
@@ -329,7 +329,9 @@
 	}
 	logger.Debugw(ctx, "omci-message-decoded:", log.Fields{"omciMsgType": omciMsg.MessageType,
 		"transCorrId": strconv.FormatInt(int64(omciMsg.TransactionID), 16), "DeviceIdent": omciMsg.DeviceIdentifier})
-	if byte(omciMsg.MessageType)&me.AK == 0 {
+	// TestResult is asynchronous indication that carries the same TID as the TestResponse.
+	// We expect to find the TID in the oo.rxSchedulerMap
+	if byte(omciMsg.MessageType)&me.AK == 0 && omciMsg.MessageType != omci.TestResultType {
 		// Not a response
 		oo.printRxMessage(ctx, rxMsg)
 		logger.Debug(ctx, "RxMsg is no Omci Response Message")
@@ -340,7 +342,6 @@
 			log.Fields{"msgType": omciMsg.MessageType, "payload": hex.EncodeToString(omciMsg.Payload),
 				"device-id": oo.deviceID})
 		return fmt.Errorf("autonomous Omci Message with TranSCorrId != 0 not acccepted %s", oo.deviceID)
-
 	}
 	//logger.Debug(ctx,"RxMsg is a Omci Response Message: try to schedule it to the requester")
 	oo.mutexRxSchedMap.Lock()
@@ -356,8 +357,12 @@
 			oo.pOnuDeviceEntry.incrementMibDataSync(ctx)
 		}
 
-		// having posted the response the request is regarded as 'done'
-		delete(oo.rxSchedulerMap, omciMsg.TransactionID)
+		// If omciMsg.MessageType is omci.TestResponseType, we still expect the TestResult OMCI message,
+		// so do not clean up the TransactionID in that case.
+		if omciMsg.MessageType != omci.TestResponseType {
+			// having posted the response the request is regarded as 'done'
+			delete(oo.rxSchedulerMap, omciMsg.TransactionID)
+		}
 		oo.mutexRxSchedMap.Unlock()
 		return nil
 	}
@@ -476,6 +481,12 @@
 }
 */
 
+// ReleaseTid releases OMCI transaction identifier from rxSchedulerMap
+func (oo *omciCC) ReleaseTid(ctx context.Context, tid uint16) {
+	logger.Debugw(ctx, "releasing tid from rxSchedulerMap", log.Fields{"tid": tid})
+	delete(oo.rxSchedulerMap, tid)
+}
+
 //Queue the OMCI Frame for a transmit to the ONU via the proxy_channel
 func (oo *omciCC) send(ctx context.Context, txFrame []byte, timeout int, retry int, highPrio bool,
 	receiveCallbackPair callbackPair) error {
@@ -2892,10 +2903,64 @@
 	return nil
 }
 
+func (oo *omciCC) sendSelfTestReq(ctx context.Context, classID me.ClassID, instdID uint16, timeout int, highPrio bool, rxChan chan Message) error {
+	tid := oo.getNextTid(highPrio)
+	logger.Debugw(ctx, "send self test request:", log.Fields{"device-id": oo.deviceID,
+		"SequNo": strconv.FormatInt(int64(tid), 16),
+		"InstId": strconv.FormatInt(int64(instdID), 16)})
+	omciLayer := &omci.OMCI{
+		TransactionID: tid,
+		MessageType:   omci.TestRequestType,
+		// DeviceIdentifier: omci.BaselineIdent,    // Optional, defaults to Baseline
+		// Length:           0x28,                                      // Optional, defaults to 40 octets
+	}
+
+	var request *omci.OpticalLineSupervisionTestRequest
+	switch classID {
+	case aniGClassID:
+		request = &omci.OpticalLineSupervisionTestRequest{
+			MeBasePacket: omci.MeBasePacket{
+				EntityClass:    classID,
+				EntityInstance: instdID,
+			},
+			SelectTest:               uint8(7), // self test
+			GeneralPurposeBuffer:     uint16(0),
+			VendorSpecificParameters: uint16(0),
+		}
+	default:
+		logger.Errorw(ctx, "unsupported class id for self test request", log.Fields{"device-id": oo.deviceID, "classID": classID})
+		return fmt.Errorf("unsupported-class-id-for-self-test-request-%v", classID)
+	}
+	// Test serialization back to former string
+	var options gopacket.SerializeOptions
+	options.FixLengths = true
+
+	buffer := gopacket.NewSerializeBuffer()
+	err := gopacket.SerializeLayers(buffer, options, omciLayer, request)
+	if err != nil {
+		logger.Errorw(ctx, "Cannot serialize self test request", log.Fields{"Err": err,
+			"device-id": oo.deviceID})
+		return err
+	}
+	outgoingPacket := buffer.Bytes()
+
+	omciRxCallbackPair := callbackPair{cbKey: tid,
+		cbEntry: callbackPairEntry{rxChan, oo.receiveOmciResponse, true},
+	}
+	err = oo.send(ctx, outgoingPacket, timeout, 0, highPrio, omciRxCallbackPair)
+	if err != nil {
+		logger.Errorw(ctx, "Cannot send self test request", log.Fields{"Err": err,
+			"device-id": oo.deviceID})
+		return err
+	}
+	logger.Debug(ctx, "send self test request done")
+	return nil
+}
+
 func isSuccessfulResponseWithMibDataSync(omciMsg *omci.OMCI, packet *gp.Packet) bool {
 	for _, v := range responsesWithMibDataSync {
 		if v == omciMsg.MessageType {
-			nextLayer, _ := omci.MsgTypeToNextLayer(v)
+			nextLayer, _ := omci.MsgTypeToNextLayer(v, false)
 			msgLayer := (*packet).Layer(nextLayer)
 			switch nextLayer {
 			case omci.LayerTypeCreateResponse:
diff --git a/internal/pkg/onuadaptercore/omci_self_test_handler.go b/internal/pkg/onuadaptercore/omci_self_test_handler.go
new file mode 100644
index 0000000..c38e89b
--- /dev/null
+++ b/internal/pkg/onuadaptercore/omci_self_test_handler.go
@@ -0,0 +1,383 @@
+/*
+ * Copyright 2021-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 adaptercoreonu provides the utility for onu devices, flows and statistics
+package adaptercoreonu
+
+import (
+	"context"
+	"fmt"
+	"github.com/looplab/fsm"
+	"github.com/opencord/omci-lib-go"
+	"github.com/opencord/omci-lib-go/generated"
+	"github.com/opencord/voltha-lib-go/v4/pkg/log"
+	"github.com/opencord/voltha-protos/v4/go/extension"
+	"sync"
+	"time"
+)
+
+const (
+	// events of Self Test FSM
+	selfTestEventTestRequest         = "selfTestEventTestRequest"
+	selfTestEventTestResponseSuccess = "selfTestEventTestResponseSuccess"
+	selfTestEventTestResultSuccess   = "selfTestEventTestResultSuccess"
+	selfTestEventAbort               = "selfTestEventAbort"
+)
+const (
+	// states of Self Test FSM
+	selfTestStNull               = "selfTestStNull"
+	selfTestStHandleSelfTestReq  = "selfTestStHandleSelfTestReq"
+	selfTestStHandleSelfTestResp = "selfTestStHandleSelfTestResp"
+	selfTestStHandleTestResult   = "selfTestStHandleTestResult"
+)
+
+const (
+	//SelfTestResponseWaitTimeout specifies timeout value waiting for self test response. Unit in seconds
+	SelfTestResponseWaitTimeout = 2
+)
+
+// We initiate an fsmCb per Self Test Request
+type fsmCb struct {
+	fsm          *AdapterFsm
+	reqMsg       extension.SingleGetValueRequest
+	respChan     chan extension.SingleGetValueResponse
+	stopOmciChan chan bool
+}
+
+type selfTestControlBlock struct {
+	pDeviceHandler *deviceHandler
+	deviceID       string
+
+	selfTestFsmMap  map[generated.ClassID]*fsmCb // The fsmCb is indexed by ME Class ID of the Test Action procedure
+	selfTestFsmLock sync.RWMutex
+
+	stopSelfTestModule chan bool
+}
+
+// newSelfTestMsgHandlerCb creates the selfTestControlBlock
+// Self Test Handler module supports sending SelfTestRequest and handling of SelfTestResponse/SelfTestResults
+// An ephemeral Self Test FSM is initiated for every Self Test request and multiple Self Tests on different
+// MEs (that support it) can be handled in parallel.
+// At the time of creating this module, only ANI-G self-test is supported.
+func newSelfTestMsgHandlerCb(ctx context.Context, dh *deviceHandler) *selfTestControlBlock {
+	selfTestCb := selfTestControlBlock{pDeviceHandler: dh}
+	selfTestCb.selfTestFsmMap = make(map[generated.ClassID]*fsmCb)
+	selfTestCb.deviceID = selfTestCb.pDeviceHandler.deviceID
+	selfTestCb.stopSelfTestModule = make(chan bool)
+
+	go selfTestCb.waitForStopSelfTestModuleSignal(ctx)
+
+	return &selfTestCb
+}
+
+func (selfTestCb *selfTestControlBlock) initiateNewSelfTestFsm(ctx context.Context, reqMsg extension.SingleGetValueRequest, commChan chan Message, classID generated.ClassID, respChan chan extension.SingleGetValueResponse) error {
+	aFsm := NewAdapterFsm("selfTestFsm", selfTestCb.deviceID, commChan)
+
+	if aFsm == nil {
+		logger.Errorw(ctx, "selfTestFsm AdapterFsm could not be instantiated!!", log.Fields{
+			"device-id": selfTestCb.deviceID})
+		return fmt.Errorf("nil-adapter-fsm")
+	}
+	// Self Test FSM related state machine
+	aFsm.pFsm = fsm.NewFSM(
+
+		selfTestStNull,
+		fsm.Events{
+			{Name: selfTestEventTestRequest, Src: []string{selfTestStNull}, Dst: selfTestStHandleSelfTestReq},
+			{Name: selfTestEventTestResponseSuccess, Src: []string{selfTestStHandleSelfTestReq}, Dst: selfTestStHandleSelfTestResp},
+			{Name: selfTestEventTestResultSuccess, Src: []string{selfTestStHandleSelfTestResp}, Dst: selfTestStNull},
+			{Name: selfTestEventAbort, Src: []string{selfTestStHandleSelfTestReq, selfTestStHandleSelfTestReq, selfTestStHandleTestResult, selfTestStNull}, Dst: selfTestStNull},
+		},
+		fsm.Callbacks{
+			"enter_state":                           func(e *fsm.Event) { aFsm.logFsmStateChange(ctx, e) },
+			"enter_" + selfTestStHandleSelfTestReq:  func(e *fsm.Event) { selfTestCb.selfTestFsmHandleSelfTestRequest(ctx, e) },
+			"enter_" + selfTestStHandleSelfTestResp: func(e *fsm.Event) { selfTestCb.selfTestFsmHandleSelfTestResponse(ctx, e) },
+		},
+	)
+	selfTestCb.selfTestFsmLock.Lock()
+	selfTestCb.selfTestFsmMap[classID] = &fsmCb{fsm: aFsm, reqMsg: reqMsg, respChan: respChan, stopOmciChan: make(chan bool)}
+	// Initiate the selfTestEventTestRequest on the FSM. Also pass the additional argument - classID.
+	// This is useful for the the FSM handler function to pull out fsmCb from the selfTestCb.selfTestFsmMap map.
+	selfTestCb.triggerFsmEvent(aFsm, selfTestEventTestRequest, classID)
+	selfTestCb.selfTestFsmLock.Unlock()
+
+	return nil
+}
+
+///// FSM Handlers
+
+func (selfTestCb *selfTestControlBlock) selfTestFsmHandleSelfTestRequest(ctx context.Context, e *fsm.Event) {
+	classID := e.Args[0].(generated.ClassID)
+	selfTestCb.selfTestFsmLock.RLock()
+	pFsmCb, ok := selfTestCb.selfTestFsmMap[classID]
+	selfTestCb.selfTestFsmLock.RUnlock()
+	if !ok {
+		// This case is impossible. Would be curious to see if this happens
+		logger.Fatalw(ctx, "class-id-not-found", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+	}
+	instKeys := selfTestCb.pDeviceHandler.pOnuOmciDevice.pOnuDB.getSortedInstKeys(ctx, classID)
+
+	// TODO: Choosing the first index from the instance keys. For ANI-G, this is fine as there is only one ANI-G instance. How do we handle and report self test for multiple instances?
+	if err := selfTestCb.pDeviceHandler.pOnuOmciDevice.PDevOmciCC.sendSelfTestReq(ctx, classID, instKeys[0], selfTestCb.pDeviceHandler.pOpenOnuAc.omciTimeout, false, pFsmCb.fsm.commChan); err != nil {
+		logger.Errorw(ctx, "error sending self test request", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+		selfTestCb.triggerFsmEvent(pFsmCb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, pFsmCb.respChan, extension.GetValueResponse_INTERNAL_ERROR, extension.GetValueResponse_ERROR)
+		return
+	}
+
+	go selfTestCb.handleOmciResponse(ctx, classID)
+}
+
+func (selfTestCb *selfTestControlBlock) selfTestFsmHandleSelfTestResponse(ctx context.Context, e *fsm.Event) {
+	classID := e.Args[0].(generated.ClassID)
+	// Pass the test result processing to another routine
+	go selfTestCb.handleOmciResponse(ctx, classID)
+
+}
+
+///// Utility functions
+
+func (selfTestCb *selfTestControlBlock) getMeClassID(ctx context.Context, reqMsg extension.SingleGetValueRequest) (generated.ClassID, error) {
+	switch reqMsg.GetRequest().GetRequest().(type) {
+	case *extension.GetValueRequest_OnuOpticalInfo:
+		return aniGClassID, nil
+	default:
+		logger.Warnw(ctx, "unsupported me class id for self test", log.Fields{"device-id": selfTestCb.deviceID})
+		return 0, fmt.Errorf("unsupported me class id for self test %v", selfTestCb.deviceID)
+	}
+}
+
+func (selfTestCb *selfTestControlBlock) triggerFsmEvent(pSelfTestFsm *AdapterFsm, event string, args ...generated.ClassID) {
+	go func() {
+		if len(args) > 0 {
+			_ = pSelfTestFsm.pFsm.Event(event, args[0])
+		} else {
+			_ = pSelfTestFsm.pFsm.Event(event)
+		}
+	}()
+}
+
+func (selfTestCb *selfTestControlBlock) submitFailureGetValueResponse(ctx context.Context, respChan chan extension.SingleGetValueResponse, errorCode extension.GetValueResponse_ErrorReason, statusCode extension.GetValueResponse_Status) {
+	singleValResp := extension.SingleGetValueResponse{
+		Response: &extension.GetValueResponse{
+			Status:    statusCode,
+			ErrReason: errorCode,
+		},
+	}
+	logger.Infow(ctx, "OMCI test response failure - pushing failure response", log.Fields{"device-id": selfTestCb.deviceID})
+	respChan <- singleValResp
+	logger.Infow(ctx, "OMCI test response failure - pushing failure response complete", log.Fields{"device-id": selfTestCb.deviceID})
+}
+
+func (selfTestCb *selfTestControlBlock) handleOmciMessage(ctx context.Context, msg OmciMessage, cb *fsmCb, classID generated.ClassID) {
+	logger.Debugw(ctx, "omci Msg", log.Fields{"device-id": selfTestCb.deviceID, "msgType": msg.OmciMsg.MessageType, "msg": msg})
+	switch msg.OmciMsg.MessageType {
+	case omci.TestResponseType:
+		selfTestCb.handleOmciTestResponse(ctx, msg, cb, classID)
+	case omci.TestResultType:
+		selfTestCb.handleOmciTestResult(ctx, msg, cb, classID)
+	default:
+		logger.Warnw(ctx, "Unknown Message Type", log.Fields{"msgType": msg.OmciMsg.MessageType})
+		selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, cb.respChan, extension.GetValueResponse_UNSUPPORTED, extension.GetValueResponse_ERROR)
+	}
+}
+
+func (selfTestCb *selfTestControlBlock) handleOmciTestResponse(ctx context.Context, msg OmciMessage, cb *fsmCb, classID generated.ClassID) {
+	msgLayer := (*msg.OmciPacket).Layer(omci.LayerTypeTestResponse)
+	if msgLayer == nil {
+		logger.Errorw(ctx, "omci Msg layer nil self test response", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+		selfTestCb.pDeviceHandler.pOnuOmciDevice.PDevOmciCC.ReleaseTid(ctx, msg.OmciMsg.TransactionID)
+		selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, cb.respChan, extension.GetValueResponse_INTERNAL_ERROR, extension.GetValueResponse_ERROR)
+		return
+	}
+	msgObj, msgOk := msgLayer.(*omci.TestResponse)
+	if !msgOk {
+		logger.Errorw(ctx, "omci Msg layer could not be detected for self test response", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+		selfTestCb.pDeviceHandler.pOnuOmciDevice.PDevOmciCC.ReleaseTid(ctx, msg.OmciMsg.TransactionID)
+		selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, cb.respChan, extension.GetValueResponse_INTERNAL_ERROR, extension.GetValueResponse_ERROR)
+		return
+	}
+	logger.Debugw(ctx, "OMCI test response Data", log.Fields{"device-id": selfTestCb.deviceID, "data-fields": msgObj})
+	if msgObj.Result == generated.Success && msgObj.EntityClass == classID {
+		logger.Infow(ctx, "OMCI test response success", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+		selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventTestResponseSuccess, classID)
+		return
+	}
+
+	logger.Infow(ctx, "OMCI test response failure", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+	selfTestCb.pDeviceHandler.pOnuOmciDevice.PDevOmciCC.ReleaseTid(ctx, msg.OmciMsg.TransactionID)
+	selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventAbort)
+	selfTestCb.submitFailureGetValueResponse(ctx, cb.respChan, extension.GetValueResponse_UNSUPPORTED, extension.GetValueResponse_ERROR)
+}
+
+func (selfTestCb *selfTestControlBlock) handleOmciTestResult(ctx context.Context, msg OmciMessage, cb *fsmCb, classID generated.ClassID) {
+	msgLayer := (*msg.OmciPacket).Layer(omci.LayerTypeTestResult)
+	if msgLayer == nil {
+		logger.Errorw(ctx, "omci Msg layer nil self test result", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+		selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, cb.respChan, extension.GetValueResponse_INTERNAL_ERROR, extension.GetValueResponse_ERROR)
+		return
+	}
+	var msgObj *omci.OpticalLineSupervisionTestResult
+	var msgOk bool
+	switch classID {
+	case aniGClassID:
+		msgObj, msgOk = msgLayer.(*omci.OpticalLineSupervisionTestResult)
+	default:
+		// We should not really land here
+		selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, cb.respChan, extension.GetValueResponse_INTERNAL_ERROR, extension.GetValueResponse_ERROR)
+		return
+	}
+	if !msgOk {
+		logger.Errorw(ctx, "omci Msg layer could not be detected for self test result", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+		selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, cb.respChan, extension.GetValueResponse_INTERNAL_ERROR, extension.GetValueResponse_ERROR)
+		return
+	}
+	logger.Debugw(ctx, "raw omci values of ani-g test result",
+		log.Fields{"device-id": selfTestCb.deviceID,
+			"power-feed-voltage": msgObj.PowerFeedVoltage,
+			"rx-power":           msgObj.ReceivedOpticalPower,
+			"tx-power":           msgObj.MeanOpticalLaunch,
+			"laser-bias-current": msgObj.LaserBiasCurrent,
+			"temperature":        msgObj.Temperature})
+	singleValResp := extension.SingleGetValueResponse{
+		Response: &extension.GetValueResponse{
+			Status: extension.GetValueResponse_OK,
+			Response: &extension.GetValueResponse_OnuOpticalInfo{
+				OnuOpticalInfo: &extension.GetOnuPonOpticalInfoResponse{
+					// OMCI representation is Volts, 2s compliment, 20mV resolution
+					PowerFeedVoltage: float32(TwosComplementToSignedInt16(msgObj.PowerFeedVoltage)) * 0.02,
+					// OMCI representation is Decibel-microwatts, 2s compliment, 0.002dB resolution
+					// Note: The OMCI table A.3.39.5 seems to be wrong about resolution. While it says the units are
+					// in Decibel-microwatts, 2s complement, 0.002 dB resolution, it actually seems to be
+					// Decibel-milliwatts, 2s complement, 0.002 dB resolution after analyzing the results from the ONUs
+					ReceivedOpticalPower: float32(TwosComplementToSignedInt16(msgObj.ReceivedOpticalPower)) * 0.002,
+					// OMCI representation is Decibel-microwatts, 2s compliment, 0.002dB resolution
+					// Same comments as in the case of ReceivedOpticalPower about resolution.
+					MeanOpticalLaunchPower: float32(TwosComplementToSignedInt16(msgObj.MeanOpticalLaunch)) * 0.002,
+					// OMCI representation is unsigned int, 2uA resolution
+					// units of gRPC interface is mA.
+					LaserBiasCurrent: float32(msgObj.LaserBiasCurrent) * 0.000002 * 1000, // multiply by 1000 to get units in mA
+					// OMCI representation is 2s complement, 1/256 degree Celsius resolution
+					Temperature: float32(TwosComplementToSignedInt16(msgObj.Temperature)) / 256.0,
+				},
+			},
+		},
+	}
+	logger.Debugw(ctx, "ani-g test result after type/value conversion",
+		log.Fields{"device-id": selfTestCb.deviceID,
+			"power-feed-voltage": singleValResp.Response.GetOnuOpticalInfo().PowerFeedVoltage,
+			"rx-power":           singleValResp.Response.GetOnuOpticalInfo().ReceivedOpticalPower,
+			"tx-power":           singleValResp.Response.GetOnuOpticalInfo().MeanOpticalLaunchPower,
+			"laser-bias-current": singleValResp.Response.GetOnuOpticalInfo().LaserBiasCurrent,
+			"temperature":        singleValResp.Response.GetOnuOpticalInfo().Temperature})
+	selfTestCb.triggerFsmEvent(cb.fsm, selfTestEventTestResultSuccess)
+	logger.Infow(ctx, "OMCI test result success - pushing results", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+	cb.respChan <- singleValResp
+	selfTestCb.selfTestRequestComplete(ctx, cb.reqMsg)
+	logger.Infow(ctx, "OMCI test result success - pushing results complete", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+}
+
+func (selfTestCb *selfTestControlBlock) handleOmciResponse(ctx context.Context, classID generated.ClassID) {
+	selfTestCb.selfTestFsmLock.RLock()
+	pFsmCb, ok := selfTestCb.selfTestFsmMap[classID]
+	selfTestCb.selfTestFsmLock.RUnlock()
+	if !ok {
+		logger.Errorw(ctx, "fsb control block unavailable", log.Fields{"device-id": selfTestCb.deviceID, "class-id": classID})
+		return
+	}
+	select {
+	case <-pFsmCb.stopOmciChan:
+		logger.Infow(ctx, "omci processing stopped", log.Fields{"device-id": selfTestCb.deviceID, "class-id": classID})
+		selfTestCb.triggerFsmEvent(pFsmCb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, pFsmCb.respChan, extension.GetValueResponse_REASON_UNDEFINED, extension.GetValueResponse_ERROR)
+	case message, ok := <-pFsmCb.fsm.commChan:
+		if !ok {
+			logger.Errorw(ctx, "Message couldn't be read from channel", log.Fields{"device-id": selfTestCb.deviceID})
+			selfTestCb.triggerFsmEvent(pFsmCb.fsm, selfTestEventAbort)
+			selfTestCb.submitFailureGetValueResponse(ctx, pFsmCb.respChan, extension.GetValueResponse_INTERNAL_ERROR, extension.GetValueResponse_ERROR)
+		}
+		logger.Debugw(ctx, "Received message on self test result channel", log.Fields{"device-id": selfTestCb.deviceID})
+
+		switch message.Type {
+		case OMCI:
+			msg, _ := message.Data.(OmciMessage)
+			selfTestCb.handleOmciMessage(ctx, msg, pFsmCb, classID)
+		default:
+			logger.Errorw(ctx, "Unknown message type received", log.Fields{"device-id": selfTestCb.deviceID, "message.Type": message.Type})
+			selfTestCb.submitFailureGetValueResponse(ctx, pFsmCb.respChan, extension.GetValueResponse_UNSUPPORTED, extension.GetValueResponse_ERROR)
+		}
+	case <-time.After(time.Duration(SelfTestResponseWaitTimeout) * time.Second):
+		logger.Errorw(ctx, "timeout waiting for test result", log.Fields{"device-id": selfTestCb.deviceID, "classID": classID})
+		selfTestCb.triggerFsmEvent(pFsmCb.fsm, selfTestEventAbort)
+		selfTestCb.submitFailureGetValueResponse(ctx, pFsmCb.respChan, extension.GetValueResponse_TIMEOUT, extension.GetValueResponse_ERROR)
+	}
+}
+
+// selfTestRequestComplete removes the fsmCb from the local cache if found
+func (selfTestCb *selfTestControlBlock) selfTestRequestComplete(ctx context.Context, reqMsg extension.SingleGetValueRequest) {
+	meClassID, err := selfTestCb.getMeClassID(ctx, reqMsg)
+	if err != nil {
+		return
+	}
+	logger.Infow(ctx, "self test req handling complete", log.Fields{"device-id": selfTestCb.deviceID, "meClassID": meClassID})
+	// Clear the fsmCb from the map
+	delete(selfTestCb.selfTestFsmMap, meClassID)
+}
+
+func (selfTestCb *selfTestControlBlock) waitForStopSelfTestModuleSignal(ctx context.Context) {
+
+	<-selfTestCb.stopSelfTestModule // block on stop signal
+
+	logger.Infow(ctx, "received stop signal - clean up start", log.Fields{"device-id": selfTestCb.deviceID})
+	selfTestCb.selfTestFsmLock.Lock()
+	for classID, fsmCb := range selfTestCb.selfTestFsmMap {
+		select {
+		case fsmCb.stopOmciChan <- true: // stop omci processing routine if one was active. It eventually aborts the fsm
+			logger.Debugw(ctx, "stopped omci processing", log.Fields{"device-id": selfTestCb.deviceID, "meClassID": classID})
+		default:
+			selfTestCb.triggerFsmEvent(fsmCb.fsm, selfTestEventAbort)
+			selfTestCb.submitFailureGetValueResponse(ctx, fsmCb.respChan, extension.GetValueResponse_REASON_UNDEFINED, extension.GetValueResponse_ERROR)
+		}
+	}
+	selfTestCb.selfTestFsmMap = make(map[generated.ClassID]*fsmCb) // reset map
+	selfTestCb.selfTestFsmLock.Unlock()
+	logger.Infow(ctx, "received stop signal - clean up end", log.Fields{"device-id": selfTestCb.deviceID})
+}
+
+//// Exported functions
+
+// selfTestRequest initiate Test Request handling procedure. The results are asynchronously conveyed on the respChan.
+// If the return from selfTestRequest is NOT nil, the caller shall not wait for async response.
+func (selfTestCb *selfTestControlBlock) SelfTestRequestStart(ctx context.Context, reqMsg extension.SingleGetValueRequest, commChan chan Message, respChan chan extension.SingleGetValueResponse) error {
+	meClassID, err := selfTestCb.getMeClassID(ctx, reqMsg)
+	if err != nil {
+		return err
+	}
+	if _, ok := selfTestCb.selfTestFsmMap[meClassID]; ok {
+		logger.Errorw(ctx, "self test already in progress for class id", log.Fields{"device-id": selfTestCb.deviceID, "class-id": meClassID})
+		return fmt.Errorf("self-test-already-in-progress-for-class-id-%v-device-id-%v", meClassID, selfTestCb.deviceID)
+	}
+	logger.Infow(ctx, "self test request initiated", log.Fields{"device-id": selfTestCb.deviceID, "meClassID": meClassID})
+	// indicates only if the FSM was initiated correctly. Response is asynchronous on respChan.
+	// If the return from here is NOT nil, the caller shall not wait for async response.
+	return selfTestCb.initiateNewSelfTestFsm(ctx, reqMsg, commChan, meClassID, respChan)
+}
diff --git a/internal/pkg/onuadaptercore/onu_metrics_manager.go b/internal/pkg/onuadaptercore/onu_metrics_manager.go
index 198d85a..8781f91 100644
--- a/internal/pkg/onuadaptercore/onu_metrics_manager.go
+++ b/internal/pkg/onuadaptercore/onu_metrics_manager.go
@@ -698,11 +698,11 @@
 					}
 				case "transmit_power_dBm":
 					if val, ok := meAttributes["TransmitOpticalLevel"]; ok && val != nil {
-						opticalMetrics[k] = float32(math.Round((float64(mm.twosComplementToSignedInt16(val.(uint16)))/500.0)*10) / 10) // convert to dBm rounded of to single decimal place
+						opticalMetrics[k] = float32(math.Round((float64(TwosComplementToSignedInt16(val.(uint16)))/500.0)*10) / 10) // convert to dBm rounded of to single decimal place
 					}
 				case "receive_power_dBm":
 					if val, ok := meAttributes["OpticalSignalLevel"]; ok && val != nil {
-						opticalMetrics[k] = float32(math.Round((float64(mm.twosComplementToSignedInt16(val.(uint16)))/500.0)*10) / 10) // convert to dBm rounded of to single decimal place
+						opticalMetrics[k] = float32(math.Round((float64(TwosComplementToSignedInt16(val.(uint16)))/500.0)*10) / 10) // convert to dBm rounded of to single decimal place
 					}
 				default:
 					// do nothing
@@ -2832,34 +2832,3 @@
 	}
 	return slice
 }
-
-func (mm *onuMetricsManager) twosComplementToSignedInt16(val uint16) int16 {
-	var uint16MsbMask uint16 = 0x8000
-	if val&uint16MsbMask == uint16MsbMask {
-		return int16(^val+1) * -1
-	}
-
-	return int16(val)
-}
-
-/* // These are need in the future
-
-func (mm *onuMetricsManager) twosComplementToSignedInt32(val uint32) int32 {
-	var uint32MsbMask uint32 = 0x80000000
-	if val & uint32MsbMask == uint32MsbMask {
-		return int32(^val + 1) * -1
-	}
-
-	return int32(val)
-}
-
-func (mm *onuMetricsManager) twosComplementToSignedInt64(val uint64) int64 {
-	var uint64MsbMask uint64 = 0x8000000000000000
-	if val & uint64MsbMask == uint64MsbMask {
-		return int64(^val + 1) * -1
-	}
-
-	return int64(val)
-}
-
-*/
diff --git a/internal/pkg/onuadaptercore/openonu.go b/internal/pkg/onuadaptercore/openonu.go
index 5cc805e..d1d6b09 100644
--- a/internal/pkg/onuadaptercore/openonu.go
+++ b/internal/pkg/onuadaptercore/openonu.go
@@ -392,13 +392,29 @@
 		if err := handler.deleteDevicePersistencyData(ctx); err != nil {
 			errorsList = append(errorsList, err)
 		}
-		handler.stopCollector <- true    // stop the metric collector routine
-		handler.stopAlarmManager <- true //stop the alarm manager.
+		select {
+		case handler.stopCollector <- true: // stop the metric collector routine
+			logger.Debugw(ctx, "sent stop signal to metric collector routine", log.Fields{"device-id": device.Id})
+		default:
+			logger.Warnw(ctx, "metric collector routine not waiting on stop signal", log.Fields{"device-id": device.Id})
+		}
+		select {
+		case handler.stopAlarmManager <- true: //stop the alarm manager.
+			logger.Debugw(ctx, "sent stop signal to alarm manager", log.Fields{"device-id": device.Id})
+		default:
+			logger.Warnw(ctx, "alarm manager not waiting on stop signal", log.Fields{"device-id": device.Id})
+		}
 		if handler.pOnuMetricsMgr != nil {
 			if err := handler.pOnuMetricsMgr.clearAllPmData(ctx); err != nil {
 				errorsList = append(errorsList, err)
 			}
 		}
+		select {
+		case handler.pSelfTestHdlr.stopSelfTestModule <- true:
+			logger.Debugw(ctx, "sent stop signal to self test handler module", log.Fields{"device-id": device.Id})
+		default:
+			logger.Warnw(ctx, "self test handler module not waiting on stop signal", log.Fields{"device-id": device.Id})
+		}
 		//don't leave any garbage - even in error case
 		oo.deleteDeviceHandlerToMap(handler)
 		if len(errorsList) > 0 {
@@ -565,6 +581,21 @@
 		switch reqType := request.GetRequest().GetRequest().(type) {
 		case *extension.GetValueRequest_UniInfo:
 			return handler.getUniPortStatus(ctx, reqType.UniInfo), nil
+		case *extension.GetValueRequest_OnuOpticalInfo:
+			commChan := make(chan Message)
+			respChan := make(chan extension.SingleGetValueResponse)
+			// Initiate the self test request
+			if err := handler.pSelfTestHdlr.SelfTestRequestStart(ctx, request, commChan, respChan); err != nil {
+				return &extension.SingleGetValueResponse{
+					Response: &extension.GetValueResponse{
+						Status:    extension.GetValueResponse_ERROR,
+						ErrReason: extension.GetValueResponse_INTERNAL_ERROR,
+					},
+				}, err
+			}
+			// The timeout handling is already implemented in omci_self_test_handler module
+			resp := <-respChan
+			return &resp, nil
 		default:
 			return postUniStatusErrResponse(extension.GetValueResponse_UNSUPPORTED), nil
 
diff --git a/internal/pkg/onuadaptercore/openonu_utils.go b/internal/pkg/onuadaptercore/openonu_utils.go
index 32eeed8..2d17b05 100644
--- a/internal/pkg/onuadaptercore/openonu_utils.go
+++ b/internal/pkg/onuadaptercore/openonu_utils.go
@@ -71,3 +71,13 @@
 	}
 	return out
 }
+
+// TwosComplementToSignedInt16 convert 2s complement to signed int16
+func TwosComplementToSignedInt16(val uint16) int16 {
+	var uint16MsbMask uint16 = 0x8000
+	if val&uint16MsbMask == uint16MsbMask {
+		return int16(^val+1) * -1
+	}
+
+	return int16(val)
+}