diff --git a/agent/Makefile.in b/agent/Makefile.in
index d9c8c5b..bd4e3da 100644
--- a/agent/Makefile.in
+++ b/agent/Makefile.in
@@ -28,13 +28,13 @@
 # sdk-all-<SDK_VER>.tar.gz - Broadcom Qumran SDK.
 # ACCTON_BAL_<BAL_VER>-<DEV_VER>.patch - Accton/Edgecore's patch.
 BAL_MAJOR_VER = 3
-BAL_MINOR_VER = 0
-BAL_REV_A_VER = 3
-BAL_REV_B_VER = 3
-BAL_API_VER = $(BAL_MAJOR_VER).$(BAL_MINOR_VER)
+BAL_MINOR_VER = 1
+BAL_REV_A_VER = 1
+BAL_REV_B_VER = 1
+BAL_API_VER = $(BAL_MAJOR_VER).$(BAL_MINOR_VER).$(BAL_REV_A_VER)
 BAL_VER = $(BAL_MAJOR_VER).$(BAL_MINOR_VER).$(BAL_REV_A_VER).$(BAL_REV_B_VER)
 SDK_VER = 6.5.13
-DEV_VER ?= 201904171432
+DEV_VER ?= 201908010203
 #
 # Version of Open Network Linux (ONL).
 ONL_KERN_VER_MAJOR = 4.14
@@ -101,15 +101,15 @@
 ##        ONL
 ##
 ##
-ONL_REPO = $(OPENOLTDEVICE)-onl
+ONL_REPO = onl
 ONL_DIR = $(BUILD_DIR)/$(ONL_REPO)
 onl:
 	if [ ! -d "$(ONL_DIR)/OpenNetworkLinux" ]; then \
 		mkdir -p $(ONL_DIR); \
 		git clone https://github.com/opencomputeproject/OpenNetworkLinux.git $(ONL_DIR)/OpenNetworkLinux; \
 		cp download/Makefile.onl $(ONL_DIR)/Makefile; \
-		install -m 755 download/build-$(OPENOLTDEVICE).sh $(ONL_DIR)/OpenNetworkLinux; \
-		make -C $(ONL_DIR) $(OPENOLTDEVICE)-$(ONL_KERN_VER_MAJOR); \
+		install -m 755 download/build-onl.sh $(ONL_DIR)/OpenNetworkLinux; \
+		make -C $(ONL_DIR) onl-$(ONL_KERN_VER_MAJOR); \
 	fi;
 onl-force:
 	make -C $(ONL_DIR) $(OPENOLTDEVICE)-$(ONL_KERN_VER_MAJOR)
@@ -127,7 +127,7 @@
 bal-api-$(BAL_API_VER):
 	if [ ! -d "$(BAL_API_DIR)" ]; then \
 		mkdir -p $(BAL_API_DIR); \
-		git clone https://github.com/balapi/bal-api-3.0.git $(BAL_API_DIR); \
+		git clone https://github.com/balapi/bal-api-$(BAL_API_VER).git $(BAL_API_DIR); \
 	fi;
 bal-api-clean:
 	rm $(BAL_API_DIR)
@@ -169,10 +169,6 @@
 	cp download/$(SDK_ZIP) $(BCM_SDK)
 	chmod -R 744 $(BAL_DIR)
 	cat download/$(ACCTON_PATCH) | patch -p1 -d $(BAL_DIR)
-	# For fixed Broadcom bug, The patch file have provided temporary change in BAL v3.0
-	# Broadcom CS8250563: Getting the wrong state using NNI interface?
-	# Broadcom CS8341292: I got error message "Failed to allocate message for NNI state change!"
-	cat download/ACCTON_BAL_$(BAL_VER)-V$(DEV_VER)_r1-bug-fixed.patch | patch -p1 -d $(BAL_DIR)
 	make -C $(BAL_DIR) BOARD=$(OPENOLTDEVICE) ONL_DIR=$(TOP_DIR)/$(ONL_DIR)/OpenNetworkLinux OPEN_SOURCE=y
 	make -C $(BAL_DIR) BOARD=$(OPENOLTDEVICE) release_board
 endif
@@ -181,7 +177,7 @@
 	make -C $(BAL_DIR) BOARD=$(OPENOLTDEVICE) release_board
 
 bal-clean:
-	make -C $(BAL_DIR) BOARD=$(OPENOLTDEVICE) clean
+	make -C $(BAL_DIR) BOARD=$(OPENOLTDEVICE) clean_all
 
 ########################################################################
 ##
@@ -259,44 +255,51 @@
 	$(CXX) $(CXXFLAGS) $(CXXFLAGSDEVICE) -I./common -c $< -o $@
 
 deb:
-	cp $(BUILD_DIR)/release_$(OPENOLTDEVICE)_V$(BAL_MAJOR_VER).$(BAL_MINOR_VER).$(BAL_REV_A_VER).$(DEV_VER).tar.gz mkdebian/debian
-	cp $(BUILD_DIR)/openolt mkdebian/debian
-	cp $(BUILD_DIR)/libgrpc.so.6 mkdebian/debian
-	cp $(BUILD_DIR)/libgrpc++.so.1 mkdebian/debian
-	cp $(BUILD_DIR)/libstdc++.so.6 mkdebian/debian
-	cp $(BUILD_DIR)/libbal_host_api.so mkdebian/debian
-	cp -a scripts/init.d mkdebian/debian
-	cd mkdebian && ./build_$(OPENOLTDEVICE)_deb.sh
-	mv *.deb $(BUILD_DIR)/openolt.deb
+	cp $(BUILD_DIR)/release_$(OPENOLTDEVICE)_V$(BAL_MAJOR_VER).$(BAL_MINOR_VER).$(BAL_REV_A_VER).$(DEV_VER).tar.gz device/$(OPENOLTDEVICE)/mkdebian/debian
+	cp $(BUILD_DIR)/openolt device/$(OPENOLTDEVICE)/mkdebian/debian
+	cp $(BUILD_DIR)/libgrpc.so.6 device/$(OPENOLTDEVICE)/mkdebian/debian
+	cp $(BUILD_DIR)/libgrpc++.so.1 device/$(OPENOLTDEVICE)/mkdebian/debian
+	cp $(BUILD_DIR)/libstdc++.so.6 device/$(OPENOLTDEVICE)/mkdebian/debian
+	cp $(BUILD_DIR)/libbal_host_api.so device/$(OPENOLTDEVICE)/mkdebian/debian
+	cp -a scripts/init.d device/$(OPENOLTDEVICE)/mkdebian/debian
+	cd device/$(OPENOLTDEVICE)/mkdebian && ./build_$(OPENOLTDEVICE)_deb.sh
+	mv device/$(OPENOLTDEVICE)/*.deb $(BUILD_DIR)/openolt_$(OPENOLTDEVICE).deb
 	make deb-cleanup
 
 src/%.o: %.cpp
 	$(CXX) -MMD -c $< -o $@
 
 deb-cleanup:
-	rm -f mkdebian/debian/$(OPENOLTDEVICE).debhelper.log
-	rm -f mkdebian/debian/$(OPENOLTDEVICE).postinst.debhelper
-	rm -f mkdebian/debian/$(OPENOLTDEVICE).postrm.debhelper
-	rm -f mkdebian/debian/$(OPENOLTDEVICE).substvars
-	rm -rf mkdebian/debian/$(OPENOLTDEVICE)/
-	rm -f mkdebian/debian/libgrpc++.so.1
-	rm -f mkdebian/debian/libgrpc.so.6
-	rm -f mkdebian/debian/openolt
-	rm -f mkdebian/debian/release_$(OPENOLTDEVICE)_V$(BAL_MAJOR_VER).$(DEV_VER).tar.gz
-	rm -rf mkdebian/debian/tmp/
-	rm -f $(OPENOLTDEVICE)_$(BAL_VER)+edgecore-V$(DEV_VER)_amd64.changes
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/$(OPENOLTDEVICE).debhelper.log
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/$(OPENOLTDEVICE).postinst.debhelper
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/$(OPENOLTDEVICE).postrm.debhelper
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/$(OPENOLTDEVICE).substvars
+	@rm -rf device/$(OPENOLTDEVICE)/mkdebian/debian/$(OPENOLTDEVICE)/
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/debhelper-build-stamp
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/libgrpc++.so.1
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/libgrpc.so.6
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/libstdc++.so.6
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/libbal_host_api.so
+	@rm -rf device/$(OPENOLTDEVICE)/mkdebian/debian/init.d/
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/openolt
+	@rm -f device/$(OPENOLTDEVICE)/mkdebian/debian/release_$(OPENOLTDEVICE)_V$(BAL_MAJOR_VER).$(BAL_MINOR_VER).$(BAL_REV_A_VER).$(DEV_VER).tar.gz
+	@rm -rf device/$(OPENOLTDEVICE)/mkdebian/debian/tmp/
+	@rm -f device/$(OPENOLTDEVICE)/$(OPENOLTDEVICE)_$(BAL_VER)+edgecore-V$(DEV_VER)_amd64.changes
 
 clean: protos-clean deb-cleanup
-	rm -f $(OBJS) $(DEPS)
-	rm -rf $(OPENOLT_PROTOS_DIR)/googleapis
-	rm -f $(BUILD_DIR)/libgrpc.so.6 $(BUILD_DIR)/libgrpc++.so.1
-	rm -f $(BUILD_DIR)/libbal_api_dist.so
-	rm -f $(BUILD_DIR)/openolt
-	rm -f $(BUILD_DIR)/bal_core_dist
-	rm -f $(BUILD_DIR)/release_$(OPENOLTDEVICE)_V$(BAL_MAJOR_VER).$(DEV_VER).tar.gz
-	rm -f $(BUILD_DIR)/openolt.deb
+	@rm -f $(OBJS) $(DEPS)
+	@rm -rf $(OPENOLT_PROTOS_DIR)/googleapis
+	@rm -f $(BUILD_DIR)/libgrpc.so.6 $(BUILD_DIR)/libgrpc++.so.1
+	@rm -f $(BUILD_DIR)/libbal_api_dist.so
+	@rm -f $(BUILD_DIR)/openolt
+	@rm -f $(BUILD_DIR)/bal_core_dist
+	@rm -f $(BUILD_DIR)/release_$(OPENOLTDEVICE)_V$(BAL_MAJOR_VER).$(DEV_VER).tar.gz
+	@rm -f $(BUILD_DIR)/openolt.deb
 
-distclean:
-	rm -rf $(BUILD_DIR)
+clean-src: protos-clean
+	@rm -f $(OBJS) $(DEPS)
+
+distclean: clean-src clean
+	@rm -rf $(BUILD_DIR)
 
 .PHONY: onl sdk bal protos prereq sim
diff --git a/agent/README.md b/agent/README.md
index 4e41fd7..1897a7a 100644
--- a/agent/README.md
+++ b/agent/README.md
@@ -73,35 +73,35 @@
 
 ## Run OpenOLT as a Linux service
 
-Rebooting the OLT (after the installation) will start bal_core_dist and openolt as init.d services:
+Rebooting the OLT (after the installation) will start dev_mgmt_daemon and openolt as init.d services:
 
-Rebooting the OLT will start the bal_core_dist and openolt services:
+Rebooting the OLT will start the dev_mgmt_daemon and openolt services:
 ```shell
 reboot
 ```
 The services can also be stopped/started manually:
 ```shell 
-service bal_core_dist stop
+service dev_mgmt_daemon stop
 service openolt stop
-service bal_core_dist start
+service dev_mgmt_daemon start
 service openolt start
 ```
 
 Check the status of the services:
 ```shell
-service bal_core_dist status
+service dev_mgmt_daemon status
 service openolt status
 ```
 
 ## Run OpenOLT in foreground
 
-Running the bal_core_dist and/or openolt services in the forground is useful for development and debugging. Make sure to first stop the services if they are running in background.
+Running the dev_mgmt_daemon and/or openolt services in the forground is useful for development and debugging. Make sure to first stop the services if they are running in background.
 
-Open a terminal and run the Broadcom BAL software (*bal_core_dist*):
+Open a terminal and run the Broadcom BAL software (*dev_mgmt_daemon*):
 
 ```shell
 cd /broadcom
-./bal_core_dist -C :55001
+./dev_mgmt_daemon -d -pcie
 ```
 
 While the first executable still runs (even in background), open another
@@ -109,7 +109,7 @@
 
 ```shell
 cd /broadcom
-./openolt -C 127.0.0.1:55001
+./openolt
 ```
 
 > **NOTE**: the two executables will remain open in the terminals, unless they
@@ -134,7 +134,7 @@
 
 ### Supported BAL API versions
 
-Currently, OpenOLT support the Broadcom BAL APIs, version *2.6.0.1*.
+Currently, OpenOLT support the Broadcom BAL APIs, version *3.1.1.1*.
 
 ### Proprietary software requirements
 
@@ -143,15 +143,12 @@
 * `SW-BCM68620_<BAL_VER>.zip` - Broadcom BAL source and Maple SDK
 * `sdk-all-<SDK_VER>.tar.gz` - Broadcom Qumran SDK
 * `ACCTON_BAL_<BAL_VER>-<ACCTON_VER>.patch` - Accton/Edgecore's patch
-* `OPENOLT_BAL_<BAL_VER>.patch` - A patch to Broadcom software to allow
-  compilation with C++ based openolt
 
 The versions currently supported by the OpenOLT agent are:
 
-* SW-BCM68620_2_6_0_1.zip
-* sdk-all-6.5.7.tar.gz
-* ACCTON_BAL_2.6.0.1-V201804301043.patch
-* OPENOLT_BAL_2.6.0.1.patch
+* SW-BCM68620_3_1_1_1.tgz
+* sdk-all-6.5.13.tar.gz
+* ACCTON_BAL_3.1.1.1-V201908010203.patch
 
 > NOTE: the repository does not contain the above four source packages.  These
 > are needed to build the OpenOLT agent executable. Contact
@@ -159,7 +156,7 @@
 
 ### System Requirements
 
-OpenOLT agent builds on *Ubuntu 14.04 LTS*.
+OpenOLT agent builds on *Ubuntu 16.04 LTS*.
 
 ### Build procedure
 
@@ -171,11 +168,11 @@
 git clone https://gerrit.opencord.org/openolt
 ```
 
-Copy the Broadcom source and patch files to the openolt/download directory:
+Copy the Broadcom source and patch files to the openolt/agent/download directory:
 
 ```shell
-cd openolt/download
-cp SW-BCM68620_2_6_0_1.zip sdk-all-6.5.7.tar.gz ACCTON_BAL_2.6.0.1-V201804301043.patch OPENOLT_BAL_2.6.0.1.patch ./download
+cd <dir containing Broadcom source and patch files>
+cp SW-BCM68620_3_1_1_1.tgz sdk-all-6.5.13.tar.gz ACCTON_BAL_3.1.1.1-V201908010203.patch <cloned openolt repo path>/agent/download
 ```
 
 Run Autoconfig to generate the appropriate makefile scaffolding for the desired target
@@ -201,7 +198,7 @@
 ```
 
 If the build process succeeds, libraries and executables will be created in the
-*openolt/build* directory.
+*openolt/agent/build* directory.
 
 Optionally, build the debian package that will be installed on the OLT.
 
@@ -210,7 +207,7 @@
 ```
 
 If the build process succeeds, the *openolt.deb* package will be created as
-well in the *openolt/build* directory.
+well in the *openolt/agent/build* directory.
 
 ### Cleanup
 
@@ -224,13 +221,13 @@
 
 ### How to change speed of ASFVOLT16 NNI interface?
 
-Auto-negotiation on the NNI (uplink) interfaces is not tested. By default, the OpenOLT agent sets the speed of the NNI interfaces to 100G. To downgrade the network interface speed to 40G, add the following lines at the end of the qax.soc (/broadcom/qax.soc) configuration file. A restart of the bal_core_dist and openolt executables is required after the change.
+Auto-negotiation on the NNI (uplink) interfaces is not tested. By default, the OpenOLT agent sets the speed of the NNI interfaces to 100G. To downgrade the network interface speed to 40G, add the following lines at the end of the qax.soc (/broadcom/qax.soc) configuration file. A restart of the dev_mgmt_daemon and openolt executables is required after the change.
 
 ```shell
 port ce128 sp=40000
 ```
 
-This change can also be made at run-time from the CLI of the bal_core_dist:
+This change can also be made at run-time from the CLI of the dev_mgmt_daemon:
 
 ```shell
 d/s/shell
@@ -284,30 +281,52 @@
 Following is an example of retrieving the interface description for PON intf_id 0 (TODO: document PON interface numbering for Edgecore OLT).
 
 ```shell
-ACC.0>b/t clear=no object=interface intf_id=0 intf_type=pon
-[-- API Start: bcmbal_stat_get --]
+BCM.0> a/t clear=no object=pon_interface sub=itu_pon_stats pon_ni=0
+[-- API Start: bcmolt_stat_get --]
 [-- API Message Data --]
-object: interface - BAL interface object
-get stat: response: OK
-key:
-   intf_id=0
-   intf_type=pon
-data:
-   rx_bytes=18473516
-   rx_packets=176416
-   rx_ucast_packets=30627
-   rx_mcast_packets=2230
-   rx_bcast_packets=143559
-   rx_error_packets=0
-   rx_unknown_protos=0
-   rx_crc_errors=0
-   bip_errors=0
-   tx_bytes=5261350
-   tx_packets=39164
-   tx_ucast_packets=30583
-   tx_mcast_packets=0
-   tx_bcast_packets=8581
-   tx_error_packets=0
+object: pon_interface- PON Network Interface.
+get stat subgroup: itu_pon_stats-ITU PON Statistics. loid: 0 response: OK
+key={pon_ni=0}
+data={
+  fec_codewords=0
+  bip_units=0
+  bip_errors=0
+  rx_gem=0
+  rx_gem_dropped=0
+  rx_gem_idle=0
+  rx_gem_corrected=0
+  rx_crc_error=0
+  rx_fragment_error=0
+  rx_packets_dropped=0
+  rx_dropped_too_short=0
+  rx_dropped_too_long=0
+  rx_key_error=0
+  rx_cpu_omci_packets_dropped=0
+  tx_ploams=157
+  rx_ploams_dropped=0
+  rx_allocations_valid=0
+  rx_allocations_invalid=0
+  rx_allocations_disabled=0
+  rx_ploams=39
+  rx_ploams_non_idle=39
+  rx_ploams_error=0
+  rx_cpu=0
+  rx_omci=0
+  rx_omci_packets_crc_error=0
+  tx_packets=0
+  tx_gem=0
+  tx_cpu=0
+  tx_omci=0
+  tx_dropped_illegal_length=0
+  tx_dropped_tpid_miss=0
+  tx_dropped_vid_miss=0
+  tx_dropped_total=0
+  rx_xgtc_headers=39
+  rx_xgtc_corrected=0
+  rx_xgtc_uncorrected=0
+  fec_codewords_uncorrected=0
+  rx_fragments_errors=0
+}
 [-- API Complete: 0 (OK) --]
 ```
 
@@ -316,34 +335,33 @@
 Following command retrieves NNI intf_id 0:
 
 ```shell
-ACC.0>b/t clear=no object=interface intf_id=0 intf_type=nniCollecting statistics
-[-- API Start: bcmbal_stat_get --]
+BCM.0> a/t clear=no object=nni_interface sub=stats id=0
+[-- API Start: bcmolt_stat_get --]
 [-- API Message Data --]
-object: interface - BAL interface object
-get stat: response: OK
-key:
-   intf_id=0
-   intf_type=nni
-data:
-   rx_bytes=8588348
-   rx_packets=69774
-   rx_ucast_packets=61189
-   rx_mcast_packets=0
-   rx_bcast_packets=8585
-   rx_error_packets=0
-   rx_unknown_protos=0
-   tx_bytes=35354878
-   tx_packets=347167
-   tx_ucast_packets=61274
-   tx_mcast_packets=4447
-   tx_bcast_packets=281446
-   tx_error_packets=0
+object: nni_interface- nni_interface.
+get stat subgroup: stats-NNI Interface Statistics. loid: 0 response: OK
+key={id=0}
+data={
+  rx_bytes=40023
+  rx_packets=283
+  rx_ucast_packets=0
+  rx_mcast_packets=283
+  rx_bcast_packets=0
+  rx_error_packets=0
+  rx_unknown_protos=0
+  tx_bytes=0
+  tx_packets=0
+  tx_ucast_packets=0
+  tx_mcast_packets=0
+  tx_bcast_packets=0
+  tx_error_packets=0
+}
 [-- API Complete: 0 (OK) --]
 ```
 
 ### How do I list flows installed in the OLT?
 
-In the bal_core_dist CLI:
+In the dev_mgmt_daemon CLI:
 
 ```shell
 > ~, Debug/, Maple/, Board/, Cld/, Transport/, Logger/, Quit
diff --git a/agent/common/core.h b/agent/common/core.h
index 8632b5b..05d6451 100644
--- a/agent/common/core.h
+++ b/agent/common/core.h
@@ -100,7 +100,6 @@
 Status Disable_();
 Status Reenable_();
 Status GetDeviceInfo_(openolt::DeviceInfo* device_info);
-Status CreateDefaultSchedQueue_(uint32_t intf_id, const std::string direction);
 Status CreateTrafficSchedulers_(const tech_profile::TrafficSchedulers *traffic_scheds);
 Status RemoveTrafficSchedulers_(const tech_profile::TrafficSchedulers *traffic_scheds);
 Status CreateTrafficQueues_(const tech_profile::TrafficQueues *traffic_queues);
diff --git a/agent/mkdebian/build_asfvolt16_deb.sh b/agent/device/asfvolt16/mkdebian/build_asfvolt16_deb.sh
similarity index 100%
rename from agent/mkdebian/build_asfvolt16_deb.sh
rename to agent/device/asfvolt16/mkdebian/build_asfvolt16_deb.sh
diff --git a/agent/device/asfvolt16/mkdebian/debian/.debhelper/generated/asfvolt16/triggers b/agent/device/asfvolt16/mkdebian/debian/.debhelper/generated/asfvolt16/triggers
new file mode 100644
index 0000000..884a015
--- /dev/null
+++ b/agent/device/asfvolt16/mkdebian/debian/.debhelper/generated/asfvolt16/triggers
@@ -0,0 +1,2 @@
+# Triggers added by dh_makeshlibs
+activate-noawait ldconfig
diff --git a/agent/mkdebian/debian/asfvolt16.install b/agent/device/asfvolt16/mkdebian/debian/asfvolt16.install
similarity index 100%
rename from agent/mkdebian/debian/asfvolt16.install
rename to agent/device/asfvolt16/mkdebian/debian/asfvolt16.install
diff --git a/agent/mkdebian/debian/asfvolt16.postinst b/agent/device/asfvolt16/mkdebian/debian/asfvolt16.postinst
similarity index 85%
rename from agent/mkdebian/debian/asfvolt16.postinst
rename to agent/device/asfvolt16/mkdebian/debian/asfvolt16.postinst
index c271f29..44693f2 100644
--- a/agent/mkdebian/debian/asfvolt16.postinst
+++ b/agent/device/asfvolt16/mkdebian/debian/asfvolt16.postinst
@@ -17,7 +17,7 @@
 
 rm -rf /opt/bcm68620
 rm -rf /broadcom
-tar zxf /tmp/release_asfvolt16_V3.0.3.201904171432.tar.gz -C / 2>&1 > /dev/null
+tar zxf /tmp/release_asfvolt16_V3.1.1.201908010203.tar.gz -C / 2>&1 > /dev/null
 grep -q '\/opt\/bcm68620\/svk_init.sh' /etc/rc.local || sed -i -e '$i \/opt\/bcm68620\/svk_init.sh' /etc/rc.local
 rm -f /run/bcm68620
 cp /tmp/init.d/dev_mgmt_daemon /etc/init.d
@@ -33,4 +33,5 @@
 mv /tmp/libstdc++.so.6 /usr/local/lib
 mv /tmp/libbal_host_api.so /usr/local/lib
 /opt/bcm68620/svk_init.sh
-ldconfig
+[ "A`cat ~/.bashrc| grep LD_LIBRARY_PATH`" = "A" ] && echo "export LD_LIBRARY_PATH=/broadcom:\$LD_LIBRARY_PATH" >> ~/.bashrc; . ~/.bashrc
+ldconfig /broadcom
diff --git a/agent/mkdebian/debian/asfvolt16.postrm b/agent/device/asfvolt16/mkdebian/debian/asfvolt16.postrm
similarity index 100%
rename from agent/mkdebian/debian/asfvolt16.postrm
rename to agent/device/asfvolt16/mkdebian/debian/asfvolt16.postrm
diff --git a/agent/mkdebian/debian/asfvolt16.preinst b/agent/device/asfvolt16/mkdebian/debian/asfvolt16.preinst
similarity index 99%
rename from agent/mkdebian/debian/asfvolt16.preinst
rename to agent/device/asfvolt16/mkdebian/debian/asfvolt16.preinst
index 9e42489..f3c0cbf 100644
--- a/agent/mkdebian/debian/asfvolt16.preinst
+++ b/agent/device/asfvolt16/mkdebian/debian/asfvolt16.preinst
@@ -24,7 +24,7 @@
 [ -n "`pidof example_user_appl`" ] && killall example_user_appl 
 [ -n "`pidof openolt`" ] && killall openolt
 rm -f /tmp/root_dev_mgmt_pid /tmp/root_dev_mgmt_init_done
-
+sleep 3
 # unload drivers
 echo "unload kernel drivers"
 [ -n "`lsmod | grep i2c_devs`" ] && rmmod i2c_devs && sleep 1
diff --git a/agent/device/asfvolt16/mkdebian/debian/changelog b/agent/device/asfvolt16/mkdebian/debian/changelog
new file mode 100644
index 0000000..26396dd
--- /dev/null
+++ b/agent/device/asfvolt16/mkdebian/debian/changelog
@@ -0,0 +1,6 @@
+asfvolt16 (3.1.1.1+edgecore-V201908010203) stable; urgency=high
+
+    * Based on code from SW-BCM68620_3_1_1_1.tgz.
+    * Merged the Accton's fixed to ACCTON_BAL_3.1.1.1-V201908010203.patch.
+
+ -- Jason Huang <jasonsc_huang@edge-core.com>  Thu, 08 AUG 2019 01:02:03 +0800
diff --git a/agent/mkdebian/debian/compat b/agent/device/asfvolt16/mkdebian/debian/compat
similarity index 100%
rename from agent/mkdebian/debian/compat
rename to agent/device/asfvolt16/mkdebian/debian/compat
diff --git a/agent/mkdebian/debian/control b/agent/device/asfvolt16/mkdebian/debian/control
similarity index 86%
rename from agent/mkdebian/debian/control
rename to agent/device/asfvolt16/mkdebian/debian/control
index 55e7db8..1c0e2bd 100644
--- a/agent/mkdebian/debian/control
+++ b/agent/device/asfvolt16/mkdebian/debian/control
@@ -9,5 +9,5 @@
 Architecture: amd64
 Depends: ${shlibs:Depends}
 Description: Broadcom OLT drivers,CLI
-  ported from Broadcom SDK-3.0.3.3 package
+  ported from Broadcom SDK-3.1.1.1 package
 
diff --git a/agent/mkdebian/debian/copyright b/agent/device/asfvolt16/mkdebian/debian/copyright
similarity index 100%
rename from agent/mkdebian/debian/copyright
rename to agent/device/asfvolt16/mkdebian/debian/copyright
diff --git a/agent/device/asfvolt16/mkdebian/debian/files b/agent/device/asfvolt16/mkdebian/debian/files
new file mode 100644
index 0000000..3f61427
--- /dev/null
+++ b/agent/device/asfvolt16/mkdebian/debian/files
@@ -0,0 +1 @@
+asfvolt16_3.1.1.1+edgecore-V201908010203_amd64.deb misc optional
diff --git a/agent/mkdebian/debian/logrotate.d/openolt b/agent/device/asfvolt16/mkdebian/debian/logrotate.d/openolt
similarity index 100%
rename from agent/mkdebian/debian/logrotate.d/openolt
rename to agent/device/asfvolt16/mkdebian/debian/logrotate.d/openolt
diff --git a/agent/mkdebian/debian/rules b/agent/device/asfvolt16/mkdebian/debian/rules
similarity index 97%
copy from agent/mkdebian/debian/rules
copy to agent/device/asfvolt16/mkdebian/debian/rules
index 73a55b1..0fa5d87 100755
--- a/agent/mkdebian/debian/rules
+++ b/agent/device/asfvolt16/mkdebian/debian/rules
@@ -26,7 +26,7 @@
 
 override_dh_auto_install: 
 	mkdir -p $(DEB_DH_INSTALL_SOURCEDIR)/tmp
-	cp -a $(CURDIR)/debian/release_asfvolt16_V3.0.3.201904171432.tar.gz $(DEB_DH_INSTALL_SOURCEDIR)/tmp
+	cp -a $(CURDIR)/debian/release_asfvolt16_V3.1.1.201908010203.tar.gz $(DEB_DH_INSTALL_SOURCEDIR)/tmp
 	cp -a $(CURDIR)/debian/libgrpc++.so.1 $(DEB_DH_INSTALL_SOURCEDIR)/tmp
 	cp -a $(CURDIR)/debian/libgrpc.so.6 $(DEB_DH_INSTALL_SOURCEDIR)/tmp
 	cp -a $(CURDIR)/debian/libstdc++.so.6 $(DEB_DH_INSTALL_SOURCEDIR)/tmp
diff --git a/agent/device/asfvolt16/vendor.h b/agent/device/asfvolt16/vendor.h
index 12cc60c..6a34814 100644
--- a/agent/device/asfvolt16/vendor.h
+++ b/agent/device/asfvolt16/vendor.h
@@ -18,5 +18,7 @@
 #ifndef __VENDOR_H__
 #define __VENDOR_H__
 #define VENDOR_ID "EdgeCore"
+#define ASFVOLT16
 #define MODEL_ID  "asfvolt16"
+#define MAX_SUPPORTED_PON 16
 #endif
diff --git a/agent/mkdebian/build_asfvolt16_deb.sh b/agent/device/asgvolt64/mkdebian/build_asgvolt64_deb.sh
similarity index 100%
copy from agent/mkdebian/build_asfvolt16_deb.sh
copy to agent/device/asgvolt64/mkdebian/build_asgvolt64_deb.sh
diff --git a/agent/device/asgvolt64/mkdebian/debian/.debhelper/generated/asgvolt64/triggers b/agent/device/asgvolt64/mkdebian/debian/.debhelper/generated/asgvolt64/triggers
new file mode 100644
index 0000000..884a015
--- /dev/null
+++ b/agent/device/asgvolt64/mkdebian/debian/.debhelper/generated/asgvolt64/triggers
@@ -0,0 +1,2 @@
+# Triggers added by dh_makeshlibs
+activate-noawait ldconfig
diff --git a/agent/mkdebian/debian/asfvolt16.install b/agent/device/asgvolt64/mkdebian/debian/asgvolt64.install
similarity index 100%
copy from agent/mkdebian/debian/asfvolt16.install
copy to agent/device/asgvolt64/mkdebian/debian/asgvolt64.install
diff --git a/agent/mkdebian/debian/asfvolt16.postinst b/agent/device/asgvolt64/mkdebian/debian/asgvolt64.postinst
similarity index 85%
copy from agent/mkdebian/debian/asfvolt16.postinst
copy to agent/device/asgvolt64/mkdebian/debian/asgvolt64.postinst
index c271f29..e9c752c 100644
--- a/agent/mkdebian/debian/asfvolt16.postinst
+++ b/agent/device/asgvolt64/mkdebian/debian/asgvolt64.postinst
@@ -17,7 +17,7 @@
 
 rm -rf /opt/bcm68620
 rm -rf /broadcom
-tar zxf /tmp/release_asfvolt16_V3.0.3.201904171432.tar.gz -C / 2>&1 > /dev/null
+tar zxf /tmp/release_asgvolt64_V3.1.1.201908010203.tar.gz -C / 2>&1 > /dev/null
 grep -q '\/opt\/bcm68620\/svk_init.sh' /etc/rc.local || sed -i -e '$i \/opt\/bcm68620\/svk_init.sh' /etc/rc.local
 rm -f /run/bcm68620
 cp /tmp/init.d/dev_mgmt_daemon /etc/init.d
@@ -33,4 +33,5 @@
 mv /tmp/libstdc++.so.6 /usr/local/lib
 mv /tmp/libbal_host_api.so /usr/local/lib
 /opt/bcm68620/svk_init.sh
-ldconfig
+[ "A`cat ~/.bashrc| grep LD_LIBRARY_PATH`" = "A" ] && echo "export LD_LIBRARY_PATH=/broadcom:\$LD_LIBRARY_PATH" >> ~/.bashrc; . ~/.bashrc
+ldconfig /broadcom
diff --git a/agent/mkdebian/debian/asfvolt16.postrm b/agent/device/asgvolt64/mkdebian/debian/asgvolt64.postrm
similarity index 100%
copy from agent/mkdebian/debian/asfvolt16.postrm
copy to agent/device/asgvolt64/mkdebian/debian/asgvolt64.postrm
diff --git a/agent/mkdebian/debian/asfvolt16.preinst b/agent/device/asgvolt64/mkdebian/debian/asgvolt64.preinst
similarity index 99%
copy from agent/mkdebian/debian/asfvolt16.preinst
copy to agent/device/asgvolt64/mkdebian/debian/asgvolt64.preinst
index 9e42489..f3c0cbf 100644
--- a/agent/mkdebian/debian/asfvolt16.preinst
+++ b/agent/device/asgvolt64/mkdebian/debian/asgvolt64.preinst
@@ -24,7 +24,7 @@
 [ -n "`pidof example_user_appl`" ] && killall example_user_appl 
 [ -n "`pidof openolt`" ] && killall openolt
 rm -f /tmp/root_dev_mgmt_pid /tmp/root_dev_mgmt_init_done
-
+sleep 3
 # unload drivers
 echo "unload kernel drivers"
 [ -n "`lsmod | grep i2c_devs`" ] && rmmod i2c_devs && sleep 1
diff --git a/agent/device/asgvolt64/mkdebian/debian/changelog b/agent/device/asgvolt64/mkdebian/debian/changelog
new file mode 100644
index 0000000..de698d3
--- /dev/null
+++ b/agent/device/asgvolt64/mkdebian/debian/changelog
@@ -0,0 +1,6 @@
+asgvolt64 (3.1.1.1+edgecore-V201908010203) stable; urgency=high
+
+    * Based on code from SW-BCM68620_3_1_1_1.tgz.
+    * Merged the Accton's fixed to ACCTON_BAL_3.1.1.1-V201908010203.patch.
+
+ -- Jason Huang <jasonsc_huang@edge-core.com>  Thu, 08 AUG 2019 01:02:03 +0800
diff --git a/agent/mkdebian/debian/compat b/agent/device/asgvolt64/mkdebian/debian/compat
similarity index 100%
copy from agent/mkdebian/debian/compat
copy to agent/device/asgvolt64/mkdebian/debian/compat
diff --git a/agent/mkdebian/debian/control b/agent/device/asgvolt64/mkdebian/debian/control
similarity index 74%
copy from agent/mkdebian/debian/control
copy to agent/device/asgvolt64/mkdebian/debian/control
index 55e7db8..535ebb1 100644
--- a/agent/mkdebian/debian/control
+++ b/agent/device/asgvolt64/mkdebian/debian/control
@@ -1,13 +1,13 @@
-Source: asfvolt16
+Source: asgvolt64
 Section: misc
 Priority: optional
 Maintainer: Jason Huang  <jasonsc_huang@edge-core.com>
 Build-Depends: debhelper (>= 4.0.0)
 Standards-Version: 3.6.2
 
-Package: asfvolt16
+Package: asgvolt64
 Architecture: amd64
 Depends: ${shlibs:Depends}
 Description: Broadcom OLT drivers,CLI
-  ported from Broadcom SDK-3.0.3.3 package
+  ported from Broadcom SDK-3.1.1.1 package
 
diff --git a/agent/mkdebian/debian/copyright b/agent/device/asgvolt64/mkdebian/debian/copyright
similarity index 100%
copy from agent/mkdebian/debian/copyright
copy to agent/device/asgvolt64/mkdebian/debian/copyright
diff --git a/agent/device/asgvolt64/mkdebian/debian/files b/agent/device/asgvolt64/mkdebian/debian/files
new file mode 100644
index 0000000..35eaf0c
--- /dev/null
+++ b/agent/device/asgvolt64/mkdebian/debian/files
@@ -0,0 +1 @@
+asgvolt64_3.1.1.1+edgecore-V201908010203_amd64.deb misc optional
diff --git a/agent/mkdebian/debian/logrotate.d/openolt b/agent/device/asgvolt64/mkdebian/debian/logrotate.d/openolt
similarity index 100%
copy from agent/mkdebian/debian/logrotate.d/openolt
copy to agent/device/asgvolt64/mkdebian/debian/logrotate.d/openolt
diff --git a/agent/mkdebian/debian/rules b/agent/device/asgvolt64/mkdebian/debian/rules
similarity index 97%
rename from agent/mkdebian/debian/rules
rename to agent/device/asgvolt64/mkdebian/debian/rules
index 73a55b1..a8b6901 100755
--- a/agent/mkdebian/debian/rules
+++ b/agent/device/asgvolt64/mkdebian/debian/rules
@@ -26,7 +26,7 @@
 
 override_dh_auto_install: 
 	mkdir -p $(DEB_DH_INSTALL_SOURCEDIR)/tmp
-	cp -a $(CURDIR)/debian/release_asfvolt16_V3.0.3.201904171432.tar.gz $(DEB_DH_INSTALL_SOURCEDIR)/tmp
+	cp -a $(CURDIR)/debian/release_asgvolt64_V3.1.1.201908010203.tar.gz $(DEB_DH_INSTALL_SOURCEDIR)/tmp
 	cp -a $(CURDIR)/debian/libgrpc++.so.1 $(DEB_DH_INSTALL_SOURCEDIR)/tmp
 	cp -a $(CURDIR)/debian/libgrpc.so.6 $(DEB_DH_INSTALL_SOURCEDIR)/tmp
 	cp -a $(CURDIR)/debian/libstdc++.so.6 $(DEB_DH_INSTALL_SOURCEDIR)/tmp
diff --git a/agent/device/asgvolt64/vendor.cc b/agent/device/asgvolt64/vendor.cc
new file mode 100644
index 0000000..12aade8
--- /dev/null
+++ b/agent/device/asgvolt64/vendor.cc
@@ -0,0 +1,22 @@
+/*
+    Copyright (C) 2018 Open Networking Foundation
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "vendor.h"
+
+void vendor_init()
+{
+}
diff --git a/agent/device/asgvolt64/vendor.h b/agent/device/asgvolt64/vendor.h
new file mode 100644
index 0000000..a0d21c3
--- /dev/null
+++ b/agent/device/asgvolt64/vendor.h
@@ -0,0 +1,24 @@
+/*
+    Copyright (C) 2018 Open Networking Foundation
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef __VENDOR_H__
+#define __VENDOR_H__
+#define VENDOR_ID "EdgeCore"
+#define ASGVOLT64
+#define MODEL_ID  "asgvolt64"
+#define MAX_SUPPORTED_PON 64
+#endif
diff --git a/agent/download/Makefile.onl b/agent/download/Makefile.onl
index 20bff23..986d9dd 100644
--- a/agent/download/Makefile.onl
+++ b/agent/download/Makefile.onl
@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-COMMIT_ID = dc0e007
+COMMIT_ID = b4af32e
 
-asfvolt16-4.14:
-	cd OpenNetworkLinux && git checkout -b $@ $(COMMIT_ID) && docker/tools/onlbuilder --non-interactive -8 -c ./build-asfvolt16.sh
+onl-4.14:
+	cd OpenNetworkLinux && git checkout -b $@ $(COMMIT_ID) && docker/tools/onlbuilder --non-interactive -8 -c ./build-onl.sh
diff --git a/agent/download/build-asfvolt16.sh b/agent/download/build-onl.sh
similarity index 100%
rename from agent/download/build-asfvolt16.sh
rename to agent/download/build-onl.sh
diff --git a/agent/mkdebian/debian/changelog b/agent/mkdebian/debian/changelog
deleted file mode 100644
index 938b033..0000000
--- a/agent/mkdebian/debian/changelog
+++ /dev/null
@@ -1,6 +0,0 @@
-asfvolt16 (3.0.3.3+edgecore-V201904171432) stable; urgency=high
-
-    * Based on code from SW-BCM68620_3_0_3_3.tgz.
-    * Merged the Accton's fixed to ACCTON_BAL_3.0.3.3-V201904171432.patch.
-
- -- Jason Huang <jasonsc_huang@edge-core.com>  Wed, 17 APR 2019 08:12:24 +0800
diff --git a/agent/mkdebian/debian/files b/agent/mkdebian/debian/files
deleted file mode 100644
index e8fd78d..0000000
--- a/agent/mkdebian/debian/files
+++ /dev/null
@@ -1 +0,0 @@
-asfvolt16_3.0.3.3+edgecore-V201904171432_amd64.deb misc optional
diff --git a/agent/scripts/init.d/dev_mgmt_daemon b/agent/scripts/init.d/dev_mgmt_daemon
index 14ef7a5..340fe53 100755
--- a/agent/scripts/init.d/dev_mgmt_daemon
+++ b/agent/scripts/init.d/dev_mgmt_daemon
@@ -24,6 +24,8 @@
 . /lib/lsb/init-functions
 
 start() {
+  /opt/bcm68620/svk_init.sh
+  sleep 2
   printf "Starting '$NAME'... "
   export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$APPDIR
   start-stop-daemon --verbose --start --chuid "$USER:$GROUP" --background --no-close --chdir "$APPDIR" --exec "$APPBIN" -- $APPARGS 2>&1 || true
diff --git a/agent/scripts/init.d/openolt b/agent/scripts/init.d/openolt
index 3c13436..69823c2 100755
--- a/agent/scripts/init.d/openolt
+++ b/agent/scripts/init.d/openolt
@@ -24,9 +24,10 @@
 . /lib/lsb/init-functions
 
 start() {
-  touch /var/log/$NAME.log
   printf "Starting '$NAME'... "
   sleep 60
+  rm -f /var/log/$NAME.log
+  touch /var/log/$NAME.log
   start-stop-daemon --verbose --start --chuid "$USER:$GROUP" --background --no-close --make-pidfile --pidfile /var/run/$NAME.pid --chdir "$APPDIR" --exec "$APPBIN" -- $APPARGS < /dev/tty1 >> /var/log/$NAME.log 2>&1 || true
   printf "done\n"
 }
@@ -49,6 +50,7 @@
     sleep 0.5
   done 
   [ -z `cat /var/run/$NAME.pid 2>/dev/null` ] || rm /var/run/$NAME.pid
+  rm -f /var/log/$NAME.log
   printf "done\n"
 }
 
diff --git a/agent/src/core.cc b/agent/src/core.cc
index 238ce96..a2da56d 100644
--- a/agent/src/core.cc
+++ b/agent/src/core.cc
@@ -20,7 +20,6 @@
 #include <string>
 
 #include "Queue.h"
-#include <iostream>
 #include <sstream>
 #include <chrono>
 #include <thread>
@@ -55,14 +54,17 @@
 dev_log_id openolt_log_id = bcm_dev_log_id_register("OPENOLT", DEV_LOG_LEVEL_INFO, DEV_LOG_ID_TYPE_BOTH);
 dev_log_id omci_log_id = bcm_dev_log_id_register("OMCI", DEV_LOG_LEVEL_INFO, DEV_LOG_ID_TYPE_BOTH);
 
-#define MAX_SUPPORTED_INTF 16
 #define BAL_RSC_MANAGER_BASE_TM_SCHED_ID 16384
 #define MAX_TM_QUEUE_ID 8192
-#define MAX_TM_SCHED_ID 4075
+#define MAX_TM_QMP_ID 16
+#define TMQ_MAP_PROFILE_SIZE 8
+#define MAX_TM_SCHED_ID 1023
+#define MAX_SUBS_TM_SCHED_ID (MAX_SUPPORTED_PON == 16 ? MAX_TM_SCHED_ID-4-16 : MAX_TM_SCHED_ID-10-64)
 #define EAP_ETHER_TYPE 34958
 #define XGS_BANDWIDTH_GRANULARITY 16000
 #define GPON_BANDWIDTH_GRANULARITY 32000
 #define FILL_ARRAY(ARRAY,START,END,VALUE) for(int i=START;i<END;ARRAY[i++]=VALUE);
+#define COUNT_OF(array) (sizeof(array) / sizeof(array[0]))
 
 #define GET_FLOW_INTERFACE_TYPE(type) \
        (type == BCMOLT_FLOW_INTERFACE_TYPE_PON) ? "PON" : \
@@ -75,7 +77,7 @@
 
 static unsigned int num_of_nni_ports = 0;
 static unsigned int num_of_pon_ports = 0;
-static std::string intf_technologies[MAX_SUPPORTED_INTF];
+static std::string intf_technologies[MAX_SUPPORTED_PON];
 static const std::string UNKNOWN_TECH("unknown");
 static const std::string MIXED_TECH("mixed");
 static std::string board_technology(UNKNOWN_TECH);
@@ -83,15 +85,20 @@
 static unsigned int OPENOLT_FIELD_LEN = 200;
 static std::string firmware_version = "Openolt.2019.07.01";
 
-const uint32_t tm_upstream_sched_id_start = 4092;
-const uint32_t tm_downstream_sched_id_start = 4076;
-//0 to 3 are default queues. Lets not use them.
-const uint32_t tm_queue_id_start = 4;
-// Upto 8 fixed Upstream. Queue id 0 to 3 are pre-created, lets not use them.
-const uint32_t us_fixed_queue_id_list[8] = {4, 5, 6, 7, 8, 9, 10, 11};
+const uint32_t tm_upstream_sched_id_start = (MAX_SUPPORTED_PON == 16 ? \
+    MAX_TM_SCHED_ID-3 : MAX_TM_SCHED_ID-9);
+const uint32_t tm_downstream_sched_id_start = (MAX_SUPPORTED_PON == 16 ? \
+    tm_upstream_sched_id_start-16 : tm_upstream_sched_id_start-64);
+
+/* Max Queue ID supported is 7 so based on priority_q configured for GEMPORTS
+in TECH PROFILE respective Queue ID from this list will be used for both
+US and DS Queues*/
+const uint32_t queue_id_list[8] = {0, 1, 2, 3, 4, 5, 6, 7};
+
 const std::string upstream = "upstream";
 const std::string downstream = "downstream";
 bcmolt_oltid dev_id = 0;
+
 /* Current session */
 static bcmcli_session *current_session;
 static bcmcli_entry *api_parent_dir;
@@ -101,29 +108,34 @@
 uint16_t flow_id_counters = 0;
 int flow_id_data[16384][2];
 
+/* QOS Type has been pre-defined as Fixed Queue but it will be updated based on number of GEMPORTS
+   associated for a given subscriber. If GEM count = 1 for a given subscriber, qos_type will be Fixed Queue
+   else Priority to Queue */
+bcmolt_egress_qos_type qos_type = BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE;
+
 State state;
 
 static std::map<uint32_t, uint32_t> flowid_to_port; // For mapping upstream flows to logical ports
 static std::map<uint32_t, uint32_t> flowid_to_gemport; // For mapping downstream flows into gemports
 static std::map<uint32_t, std::set<uint32_t> > port_to_flows; // For mapping logical ports to downstream flows
 
-// This represents the Key to 'queue_map' map.
-// Represents (pon_intf_id, onu_id, uni_id, gemport_id, direction)
-typedef std::tuple<uint32_t, uint32_t, uint32_t, uint32_t, std::string> queue_map_key_tuple;
-// 'queue_map' maps queue_map_key_tuple to downstream queue id present
-// on the Subscriber Scheduler
-static std::map<queue_map_key_tuple, int> queue_map;
-// This represents the Key to 'sched_map' map.
-// Represents (pon_intf_id, onu_id, uni_id, direction)
-
+/* This represents the Key to 'sched_map' map.
+ Represents (pon_intf_id, onu_id, uni_id, direction) */
 typedef std::tuple<uint32_t, uint32_t, uint32_t, std::string> sched_map_key_tuple;
-// 'sched_map' maps sched_map_key_tuple to DBA (Upstream) or
-// Subscriber (Downstream) Scheduler ID
+/* 'sched_map' maps sched_map_key_tuple to DBA (Upstream) or
+ Subscriber (Downstream) Scheduler ID */
 static std::map<sched_map_key_tuple, int> sched_map;
 
+/* This represents the Key to 'sched_qmp_id_map' map.
+Represents (sched_id, pon_intf_id, onu_id, uni_id) */
+typedef std::tuple<uint32_t, uint32_t, uint32_t, uint32_t> sched_qmp_id_map_key_tuple;
+/* 'sched_qmp_id_map' maps sched_qmp_id_map_key_tuple to TM Queue Mapping Profile ID */
+static std::map<sched_qmp_id_map_key_tuple, int> sched_qmp_id_map;
+/* 'qmp_id_to_qmp_map' maps TM Queue Mapping Profile ID to TM Queue Mapping Profile */
+static std::map<int, std::vector < uint32_t > > qmp_id_to_qmp_map;
 
-std::bitset<MAX_TM_QUEUE_ID> tm_queue_bitset;
 std::bitset<MAX_TM_SCHED_ID> tm_sched_bitset;
+std::bitset<MAX_TM_QMP_ID> tm_qmp_bitset;
 
 static bcmos_fastlock data_lock;
 
@@ -138,6 +150,8 @@
 static bcmos_errno CreateQueue(std::string direction, uint32_t access_intf_id, uint32_t onu_id, uint32_t uni_id, \
                                uint32_t priority, uint32_t gemport_id);
 static bcmos_errno RemoveQueue(std::string direction, int intf_id, int onu_id, int uni_id, uint32_t port_no, int alloc_id);
+static bcmos_errno CreateDefaultSched(uint32_t intf_id, const std::string direction);
+static bcmos_errno CreateDefaultQueue(uint32_t intf_id, const std::string direction);
 
 uint16_t get_dev_id(void) {
     return dev_id;
@@ -167,90 +181,6 @@
 }
 
 /**
-* Gets a unique tm_queue_id for a given intf_id, onu_id, uni_id, gemport_id, direction
-* The tm_queue_id is locally cached in a map, so that it can rendered when necessary.
-* VOLTHA replays whole configuration on OLT reboot, so caching locally is not a problem
-*
-* @param intf_id NNI or PON intf ID
-* @param onu_id ONU ID
-* @param uni_id UNI ID
-* @param gemport_id GEM Port ID
-* @param direction Upstream or downstream
-*
-* @return tm_queue_id
-*/
-int get_tm_queue_id(int intf_id, int onu_id, int uni_id, int gemport_id, std::string direction) {
-    queue_map_key_tuple key(intf_id, onu_id, uni_id, gemport_id, direction);
-    int queue_id = -1;
-
-    std::map<queue_map_key_tuple, int>::const_iterator it = queue_map.find(key);
-    if (it != queue_map.end()) {
-        queue_id = it->second;
-    }
-    if (queue_id != -1) {
-        return queue_id;
-    }
-
-    bcmos_fastlock_lock(&data_lock);
-    // Complexity of O(n). Is there better way that can avoid linear search?
-    for (queue_id = 0; queue_id < MAX_TM_QUEUE_ID; queue_id++) {
-        if (tm_queue_bitset[queue_id] == 0) {
-            tm_queue_bitset[queue_id] = 1;
-            break;
-        }
-    }
-    bcmos_fastlock_unlock(&data_lock, 0);
-
-    if (queue_id < MAX_TM_QUEUE_ID) {
-        bcmos_fastlock_lock(&data_lock);
-        queue_map[key] = queue_id;
-        bcmos_fastlock_unlock(&data_lock, 0);
-        return queue_id;
-    } else {
-        return -1;
-    }
-}
-
-/**
-* Update tm_queue_id for a given intf_id, onu_id, uni_id, gemport_id, direction
-*
-* @param intf_id NNI or PON intf ID
-* @param onu_id ONU ID
-* @param uni_id UNI ID
-* @param gemport_id GEM Port ID
-* @param direction Upstream or downstream
-* @param tm_queue_id tm_queue_id
-*/
-void update_tm_queue_id(int pon_intf_id, int onu_id, int uni_id, int gemport_id, std::string direction,
-                                  uint32_t queue_id) {
-    queue_map_key_tuple key(pon_intf_id, onu_id, uni_id, gemport_id, direction);
-    bcmos_fastlock_lock(&data_lock);
-    queue_map[key] = queue_id;
-    bcmos_fastlock_unlock(&data_lock, 0);
-}
-
-/**
-* Free tm_queue_id for a given intf_id, onu_id, uni_id, gemport_id, direction
-*
-* @param intf_id NNI or PON intf ID
-* @param onu_id ONU ID
-* @param uni_id UNI ID
-* @param gemport_id GEM Port ID
-* @param direction Upstream or downstream
-*/
-void free_tm_queue_id(int pon_intf_id, int onu_id, int uni_id, int gemport_id, std::string direction) {
-    queue_map_key_tuple key(pon_intf_id, onu_id, uni_id, gemport_id, direction);
-    std::map<queue_map_key_tuple, int>::const_iterator it;
-    bcmos_fastlock_lock(&data_lock);
-    it = queue_map.find(key);
-    if (it != queue_map.end()) {
-        tm_queue_bitset[it->second] = 0;
-        queue_map.erase(it);
-    }
-    bcmos_fastlock_unlock(&data_lock, 0);
-}
-
-/**
 * Gets a unique tm_sched_id for a given intf_id, onu_id, uni_id, gemport_id, direction
 * The tm_sched_id is locally cached in a map, so that it can rendered when necessary.
 * VOLTHA replays whole configuration on OLT reboot, so caching locally is not a problem
@@ -318,12 +248,231 @@
 
 bool is_tm_sched_id_present(int pon_intf_id, int onu_id, int uni_id, std::string direction) {
     sched_map_key_tuple key(pon_intf_id, onu_id, uni_id, direction);
-    return sched_map.count(key) > 0 ? true: false;
+    std::map<sched_map_key_tuple, int>::const_iterator it = sched_map.find(key);
+    if (it != sched_map.end()) {
+        return true;
+    }
+    return false;
 }
 
-bool is_tm_queue_id_present(int pon_intf_id, int onu_id, int uni_id, int gemport_id, std::string direction) {
-    queue_map_key_tuple key(pon_intf_id, onu_id, uni_id, gemport_id, direction);
-    return queue_map.count(key) > 0 ? true: false;
+/**
+* Check whether given two tm qmp profiles are equal or not
+*
+* @param tmq_map_profileA <vector> TM QUEUE MAPPING PROFILE
+* @param tmq_map_profileB <vector> TM QUEUE MAPPING PROFILE
+*
+* @return boolean, true if given tmq_map_profiles are equal else false
+*/
+
+bool check_tm_qmp_equality(std::vector<uint32_t> tmq_map_profileA, std::vector<uint32_t> tmq_map_profileB) {
+    for (uint32_t i = 0; i < TMQ_MAP_PROFILE_SIZE; i++) {
+        if (tmq_map_profileA[i] != tmq_map_profileB[i]) {
+            return false;
+        }
+    }
+    return true;
+}
+
+/**
+* Modifies given queues_pbit_map to parsable format
+* e.g: Modifes "0b00000101" to "10100000"
+*
+* @param queues_pbit_map PBIT MAP configured for each GEM in TECH PROFILE
+* @param size Queue count
+*
+* @return string queues_pbit_map
+*/
+std::string* get_valid_queues_pbit_map(std::string *queues_pbit_map, uint32_t size) {
+    for(uint32_t i=0; i < size; i++) {
+        /* Deletes 2 characters from index number 0 */
+        queues_pbit_map[i].erase(0, 2);
+        std::reverse(queues_pbit_map[i].begin(), queues_pbit_map[i].end());
+    }
+    return queues_pbit_map;
+}
+
+/**
+* Creates TM QUEUE MAPPING PROFILE for given queues_pbit_map and queues_priority_q
+*
+* @param queues_pbit_map PBIT MAP configured for each GEM in TECH PROFILE
+* @param queues_priority_q PRIORITY_Q configured for each GEM in TECH PROFILE
+* @param size Queue count
+*
+* @return <vector> TM QUEUE MAPPING PROFILE
+*/
+std::vector<uint32_t> get_tmq_map_profile(std::string *queues_pbit_map, uint32_t *queues_priority_q, uint32_t size) {
+    std::vector<uint32_t> tmq_map_profile(8,0);
+
+    for(uint32_t i=0; i < size; i++) {
+        for (uint32_t j = 0; j < queues_pbit_map[i].size(); j++) {
+            if (queues_pbit_map[i][j]=='1') {
+                tmq_map_profile.at(j) = queue_id_list[queues_priority_q[i]];
+            }
+        }
+    }
+    return tmq_map_profile;
+}
+
+/**
+* Gets corresponding tm_qmp_id for a given tmq_map_profile
+*
+* @param <vector> TM QUEUE MAPPING PROFILE
+*
+* @return tm_qmp_id
+*/
+int get_tm_qmp_id(std::vector<uint32_t> tmq_map_profile) {
+    int tm_qmp_id = -1;
+
+    std::map<int, std::vector < uint32_t > >::const_iterator it = qmp_id_to_qmp_map.begin();
+    while(it != qmp_id_to_qmp_map.end()) {
+        if(check_tm_qmp_equality(tmq_map_profile, it->second)) {
+            tm_qmp_id = it->first;
+            break;
+        }
+        it++;
+    }
+    return tm_qmp_id;
+}
+
+/**
+* Updates sched_qmp_id_map with given sched_id, pon_intf_id, onu_id, uni_id, tm_qmp_id
+*
+* @param upstream/downstream sched_id
+* @param PON intf ID
+* @param onu_id ONU ID
+* @param uni_id UNI ID
+* @param tm_qmp_id TM QUEUE MAPPING PROFILE ID
+*/
+void update_sched_qmp_id_map(uint32_t sched_id,uint32_t pon_intf_id, uint32_t onu_id, \
+                             uint32_t uni_id, int tm_qmp_id) {
+   bcmos_fastlock_lock(&data_lock);
+   sched_qmp_id_map_key_tuple key(sched_id, pon_intf_id, onu_id, uni_id);
+   sched_qmp_id_map.insert(make_pair(key, tm_qmp_id));
+   bcmos_fastlock_unlock(&data_lock, 0);
+}
+
+/**
+* Gets corresponding tm_qmp_id for a given sched_id, pon_intf_id, onu_id, uni_id
+*
+* @param upstream/downstream sched_id
+* @param PON intf ID
+* @param onu_id ONU ID
+* @param uni_id UNI ID
+*
+* @return tm_qmp_id
+*/
+int get_tm_qmp_id(uint32_t sched_id,uint32_t pon_intf_id, uint32_t onu_id, uint32_t uni_id) {
+    sched_qmp_id_map_key_tuple key(sched_id, pon_intf_id, onu_id, uni_id);
+    int tm_qmp_id = -1;
+
+    std::map<sched_qmp_id_map_key_tuple, int>::const_iterator it = sched_qmp_id_map.find(key);
+    if (it != sched_qmp_id_map.end()) {
+        tm_qmp_id = it->second;
+    }
+    return tm_qmp_id;
+}
+
+/**
+* Gets a unique tm_qmp_id for a given tmq_map_profile
+* The tm_qmp_id is locally cached in a map, so that it can be rendered when necessary.
+* VOLTHA replays whole configuration on OLT reboot, so caching locally is not a problem
+*
+* @param upstream/downstream sched_id
+* @param PON intf ID
+* @param onu_id ONU ID
+* @param uni_id UNI ID
+* @param <vector> TM QUEUE MAPPING PROFILE
+*
+* @return tm_qmp_id
+*/
+int get_tm_qmp_id(uint32_t sched_id,uint32_t pon_intf_id, uint32_t onu_id, uint32_t uni_id, \
+                  std::vector<uint32_t> tmq_map_profile) {
+    int tm_qmp_id;
+
+    bcmos_fastlock_lock(&data_lock);
+    /* Complexity of O(n). Is there better way that can avoid linear search? */
+    for (tm_qmp_id = 0; tm_qmp_id < MAX_TM_QMP_ID; tm_qmp_id++) {
+        if (tm_qmp_bitset[tm_qmp_id] == 0) {
+            tm_qmp_bitset[tm_qmp_id] = 1;
+            break;
+        }
+    }
+    bcmos_fastlock_unlock(&data_lock, 0);
+
+    if (tm_qmp_id < MAX_TM_QMP_ID) {
+        bcmos_fastlock_lock(&data_lock);
+        qmp_id_to_qmp_map.insert(make_pair(tm_qmp_id, tmq_map_profile));
+        bcmos_fastlock_unlock(&data_lock, 0);
+        update_sched_qmp_id_map(sched_id, pon_intf_id, onu_id, uni_id, tm_qmp_id);
+        return tm_qmp_id;
+    } else {
+        return -1;
+    }
+}
+
+/**
+* Free tm_qmp_id for a given sched_id, pon_intf_id, onu_id, uni_id
+*
+* @param upstream/downstream sched_id
+* @param PON intf ID
+* @param onu_id ONU ID
+* @param uni_id UNI ID
+* @param tm_qmp_id TM QUEUE MAPPING PROFILE ID
+*
+* @return boolean, true if no more reference for TM QMP else false
+*/
+bool free_tm_qmp_id(uint32_t sched_id,uint32_t pon_intf_id, uint32_t onu_id, \
+                    uint32_t uni_id, int tm_qmp_id) {
+    bool result;
+    sched_qmp_id_map_key_tuple key(sched_id, pon_intf_id, onu_id, uni_id);
+    std::map<sched_qmp_id_map_key_tuple, int>::const_iterator it = sched_qmp_id_map.find(key);
+    bcmos_fastlock_lock(&data_lock);
+    if (it != sched_qmp_id_map.end()) {
+        sched_qmp_id_map.erase(it);
+    }
+    bcmos_fastlock_unlock(&data_lock, 0);
+
+    uint32_t tm_qmp_ref_count = 0;
+    std::map<sched_qmp_id_map_key_tuple, int>::const_iterator it2 = sched_qmp_id_map.begin();
+    while(it2 != sched_qmp_id_map.end()) {
+        if(it2->second == tm_qmp_id) {
+            tm_qmp_ref_count++;
+        }
+        it2++;
+    }
+
+    if (tm_qmp_ref_count == 0) {
+        std::map<int, std::vector < uint32_t > >::const_iterator it3 = qmp_id_to_qmp_map.find(tm_qmp_id);
+        if (it3 != qmp_id_to_qmp_map.end()) {
+            bcmos_fastlock_lock(&data_lock);
+            tm_qmp_bitset[tm_qmp_id] = 0;
+            qmp_id_to_qmp_map.erase(it3);
+            bcmos_fastlock_unlock(&data_lock, 0);
+            OPENOLT_LOG(INFO, openolt_log_id, "Reference count for tm qmp profile id %d is : %d. So clearing it\n", \
+                        tm_qmp_id, tm_qmp_ref_count);
+            result = true;
+        }
+    } else {
+        OPENOLT_LOG(INFO, openolt_log_id, "Reference count for tm qmp profile id %d is : %d. So not clearing it\n", \
+                    tm_qmp_id, tm_qmp_ref_count);
+        result = false;
+    }
+    return result;
+}
+
+/**
+* Returns Scheduler/Queue direction as string
+*
+* @param direction as specified in tech_profile.proto
+*/
+std::string GetDirection(int direction) {
+    switch (direction)
+    {
+        case tech_profile::Direction::UPSTREAM: return upstream;
+        case tech_profile::Direction::DOWNSTREAM: return downstream;
+        default: OPENOLT_LOG(ERROR, openolt_log_id, "direction-not-supported %d\n", direction);
+                 return "direction-not-supported";
+    }
 }
 
 inline const char *get_flow_acton_command(uint32_t command) {
@@ -645,8 +794,14 @@
                     bcmolt_device_key key = {.device_id = dev};
                     bcmolt_device_connect oper;
                     BCMOLT_OPER_INIT(&oper, device, connect, key);
-                    BCMOLT_MSG_FIELD_SET(&oper, inni_config.mode, BCMOLT_INNI_MODE_ALL_10_G_XFI);
-                    BCMOLT_MSG_FIELD_SET (&oper, system_mode, BCMOLT_SYSTEM_MODE_XGS__2_X);
+                    if (MODEL_ID == "asfvolt16") {
+                        BCMOLT_MSG_FIELD_SET(&oper, inni_config.mode, BCMOLT_INNI_MODE_ALL_10_G_XFI);
+                        BCMOLT_MSG_FIELD_SET (&oper, system_mode, BCMOLT_SYSTEM_MODE_XGS__2_X);
+                    } else if (MODEL_ID == "asgvolt64") {
+                        BCMOLT_MSG_FIELD_SET(&oper, inni_config.mode, BCMOLT_INNI_MODE_ALL_10_G_XFI);
+                        BCMOLT_MSG_FIELD_SET(&oper, inni_config.mux, BCMOLT_INNI_MUX_FOUR_TO_ONE);
+                        BCMOLT_MSG_FIELD_SET (&oper, system_mode, BCMOLT_SYSTEM_MODE_GPON__16_X);
+                    }
                     err = bcmolt_oper_submit(dev_id, &oper.hdr);
                     if (err) 
                         OPENOLT_LOG(ERROR, openolt_log_id, "Enable PON deivce %d failed\n", dev);
@@ -897,6 +1052,8 @@
                     return flow_cfg.data.egress_qos.u.tc_to_queue.tc_to_queue_id;
                 case BCMOLT_EGRESS_QOS_TYPE_PBIT_TO_TC:
                     return flow_cfg.data.egress_qos.u.pbit_to_tc.tc_to_queue_id;
+                case BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE:
+                    return flow_cfg.data.egress_qos.u.priority_to_queue.tm_q_set_id;
                 case BCMOLT_EGRESS_QOS_TYPE_NONE:
                 default:
                     return -1;
@@ -1006,7 +1163,8 @@
     else {
         OPENOLT_LOG(INFO, openolt_log_id, "Successfully enabled PON interface: %d\n", intf_id);
         OPENOLT_LOG(INFO, openolt_log_id, "Initializing tm sched creation for PON interface: %d\n", intf_id);
-        CreateDefaultSchedQueue_(intf_id, downstream);
+        CreateDefaultSched(intf_id, downstream);
+        CreateDefaultQueue(intf_id, downstream);
     }
 
     return Status::OK;
@@ -1174,7 +1332,8 @@
         if (set_state && state == BCMOLT_INTERFACE_STATE_ACTIVE_WORKING) {
             OPENOLT_LOG(INFO, openolt_log_id, "NNI interface: %d already enabled\n", intf_id);
             OPENOLT_LOG(INFO, openolt_log_id, "Initializing tm sched creation for NNI interface: %d\n", intf_id);
-            CreateDefaultSchedQueue_(intf_id, upstream);
+            CreateDefaultSched(intf_id, upstream);
+            CreateDefaultQueue(intf_id, upstream);
             return Status::OK;
         } else if (!set_state && state == BCMOLT_INTERFACE_STATE_INACTIVE) {
             OPENOLT_LOG(INFO, openolt_log_id, "NNI interface: %d already disabled\n", intf_id);
@@ -1200,7 +1359,8 @@
         OPENOLT_LOG(INFO, openolt_log_id, "Successfully %s NNI interface: %d\n", (set_state)?"enable":"disable", intf_id);
         if (set_state) {
             OPENOLT_LOG(INFO, openolt_log_id, "Initializing tm sched creation for NNI interface: %d\n", intf_id);
-            CreateDefaultSchedQueue_(intf_id, upstream);
+            CreateDefaultSched(intf_id, upstream);
+            CreateDefaultQueue(intf_id, upstream);
         }
     }
 
@@ -1251,12 +1411,16 @@
     memcpy(serial_number.vendor_id.arr, vendor_id, 4);
     memcpy(serial_number.vendor_specific.arr, vendor_specific, 4);
     BCMOLT_CFG_INIT(&onu_cfg, onu, onu_key);
-    BCMOLT_MSG_FIELD_SET(&onu_cfg, onu_rate, BCMOLT_ONU_RATE_RATE_10G_DS_10G_US);
     BCMOLT_MSG_FIELD_SET(&onu_cfg, itu.serial_number, serial_number);
     BCMOLT_MSG_FIELD_SET(&onu_cfg, itu.auto_learning, BCMOS_TRUE);
     /*set burst and data profiles to fec disabled*/
-    BCMOLT_MSG_FIELD_SET(&onu_cfg, itu.xgpon.ranging_burst_profile, 0);
-    BCMOLT_MSG_FIELD_SET(&onu_cfg, itu.xgpon.data_burst_profile, 1);
+    if (board_technology == "XGS-PON") {
+        BCMOLT_MSG_FIELD_SET(&onu_cfg, itu.xgpon.ranging_burst_profile, 2);
+        BCMOLT_MSG_FIELD_SET(&onu_cfg, itu.xgpon.data_burst_profile, 1);
+    } else if (board_technology == "GPON") {
+        BCMOLT_MSG_FIELD_SET(&onu_cfg, itu.gpon.ds_ber_reporting_interval, 1000000);
+        BCMOLT_MSG_FIELD_SET(&onu_cfg, itu.gpon.omci_port_id, onu_id);
+    }
     err = bcmolt_cfg_set(dev_id, &onu_cfg.hdr);
     if (err != BCM_ERR_OK) {
         OPENOLT_LOG(ERROR, openolt_log_id, "Failed to set activate ONU %d on PON %d, err %d\n", onu_id, intf_id, err);
@@ -1613,8 +1777,7 @@
     bcmolt_classifier c_val = { };
     bcmolt_action a_val = { };
     bcmolt_tm_queue_ref tm_val = { };
-    //Pre-defined Fixed Queue, It decides the type by caller, TODO
-    bcmolt_egress_qos_type qos_type = BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE;
+    int tm_qmp_id, tm_q_set_id;
 
     key.flow_id = flow_id;
     if (flow_type.compare(upstream) == 0 ) {
@@ -1809,37 +1972,80 @@
     }
 
     if ((access_intf_id >= 0) && (onu_id >= 0)) {
-        if (key.flow_type == BCMOLT_FLOW_TYPE_DOWNSTREAM) {
-            if (single_tag && ether_type == EAP_ETHER_TYPE) {
-                tm_val.sched_id = get_default_tm_sched_id(access_intf_id, downstream);
-                tm_val.queue_id = 0;
-            } else {
-                tm_val.sched_id = get_tm_sched_id(access_intf_id, onu_id, uni_id, downstream); // Subscriber Scheduler
-                tm_val.queue_id = get_tm_queue_id(access_intf_id, onu_id, uni_id, gemport_id, downstream);
-            }
-            OPENOLT_LOG(DEBUG, openolt_log_id, "direction = %s, queue_id = %d, sched_id = %d, intf_type %s\n", \
-                downstream.c_str(), tm_val.queue_id, tm_val.sched_id, GET_FLOW_INTERFACE_TYPE(cfg.data.ingress_intf.intf_type));
+        if(single_tag && ether_type == EAP_ETHER_TYPE) {
+            tm_val.sched_id = (flow_type.compare(upstream) == 0) ? \
+                get_default_tm_sched_id(network_intf_id, upstream) : \
+                get_default_tm_sched_id(access_intf_id, downstream);
+            tm_val.queue_id = 0;
 
-            BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.type, qos_type);
+            BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.type, BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE);
             BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.tm_sched.id, tm_val.sched_id);
             BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.u.fixed_queue.queue_id, tm_val.queue_id);
-        } else if (key.flow_type == BCMOLT_FLOW_TYPE_UPSTREAM) {
-            /* removed by BAL v3.0. N/A - Alloc ID is out of the scope of BAL. Used for OMCI only.
-               bcmbal_tm_sched_id val1;
-               val1 = get_tm_sched_id(access_intf_id, onu_id, uni_id, upstream); // DBA Scheduler ID
-               BCMBAL_CFG_PROP_SET(&cfg, flow, dba_tm_sched_id, val1);
-             */
-             tm_val.sched_id = get_default_tm_sched_id(network_intf_id, upstream); // NNI Scheduler ID
-             tm_val.queue_id = get_tm_queue_id(access_intf_id, onu_id, uni_id, gemport_id, upstream); // Queue on NNI
-             OPENOLT_LOG(DEBUG, openolt_log_id, "direction = %s, queue_id = %d, sched_id = %d, intf_type %s\n", \
-             upstream.c_str(), tm_val.queue_id, tm_val.sched_id, GET_FLOW_INTERFACE_TYPE(cfg.data.ingress_intf.intf_type));
-             BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.type, qos_type);
-             BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.tm_sched.id, tm_val.sched_id);
-             BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.u.fixed_queue.queue_id, tm_val.queue_id);
+
+            OPENOLT_LOG(DEBUG, openolt_log_id, "direction = %s, queue_id = %d, sched_id = %d, intf_type %s\n", \
+                flow_type.c_str(), tm_val.queue_id, tm_val.sched_id, \
+                GET_FLOW_INTERFACE_TYPE(cfg.data.ingress_intf.intf_type));
+        } else {
+            if (key.flow_type == BCMOLT_FLOW_TYPE_DOWNSTREAM) {
+                tm_val.sched_id = get_tm_sched_id(access_intf_id, onu_id, uni_id, downstream);
+
+                    if (qos_type == BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE) {
+                        // Queue 0 on DS subscriber scheduler
+                        tm_val.queue_id = 0;
+
+                        BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.type, qos_type);
+                        BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.tm_sched.id, tm_val.sched_id);
+                        BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.u.fixed_queue.queue_id, tm_val.queue_id);
+
+                        OPENOLT_LOG(DEBUG, openolt_log_id, "direction = %s, queue_id = %d, sched_id = %d, intf_type %s\n", \
+                            downstream.c_str(), tm_val.queue_id, tm_val.sched_id, \
+                            GET_FLOW_INTERFACE_TYPE(cfg.data.ingress_intf.intf_type));
+
+                    } else if (qos_type == BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE) {
+                        /* Fetch TM QMP ID mapped to DS subscriber scheduler */
+                        tm_qmp_id = tm_q_set_id = get_tm_qmp_id(tm_val.sched_id, access_intf_id, onu_id, uni_id);
+
+                        BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.type, qos_type);
+                        BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.tm_sched.id, tm_val.sched_id);
+                        BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.u.priority_to_queue.tm_qmp_id, tm_qmp_id);
+                        BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.u.priority_to_queue.tm_q_set_id, tm_q_set_id);
+
+                        OPENOLT_LOG(DEBUG, openolt_log_id, "direction = %s, q_set_id = %d, sched_id = %d, intf_type %s\n", \
+                            downstream.c_str(), tm_q_set_id, tm_val.sched_id, \
+                            GET_FLOW_INTERFACE_TYPE(cfg.data.ingress_intf.intf_type));
+                    }
+            } else if (key.flow_type == BCMOLT_FLOW_TYPE_UPSTREAM) {
+                // NNI Scheduler ID
+                tm_val.sched_id = get_default_tm_sched_id(network_intf_id, upstream);
+                if (qos_type == BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE) {
+                    // Queue 0 on NNI scheduler
+                    tm_val.queue_id = 0;
+                    BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.type, qos_type);
+                    BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.tm_sched.id, tm_val.sched_id);
+                    BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.u.fixed_queue.queue_id, tm_val.queue_id);
+
+                    OPENOLT_LOG(DEBUG, openolt_log_id, "direction = %s, queue_id = %d, sched_id = %d, intf_type %s\n", \
+                        upstream.c_str(), tm_val.queue_id, tm_val.sched_id, \
+                        GET_FLOW_INTERFACE_TYPE(cfg.data.ingress_intf.intf_type));
+
+                } else if (qos_type == BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE) {
+                    /* Fetch TM QMP ID mapped to US NNI scheduler */
+                    tm_qmp_id = tm_q_set_id = get_tm_qmp_id(tm_val.sched_id, access_intf_id, onu_id, uni_id);
+                    BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.type, qos_type);
+                    BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.tm_sched.id, tm_val.sched_id);
+                    BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.u.priority_to_queue.tm_qmp_id, tm_qmp_id);
+                    BCMOLT_MSG_FIELD_SET(&cfg , egress_qos.u.priority_to_queue.tm_q_set_id, tm_q_set_id);
+
+                    OPENOLT_LOG(DEBUG, openolt_log_id, "direction = %s, q_set_id = %d, sched_id = %d, intf_type %s\n", \
+                        upstream.c_str(), tm_q_set_id, tm_val.sched_id, \
+                        GET_FLOW_INTERFACE_TYPE(cfg.data.ingress_intf.intf_type));
+                }
+            }
         }
     }
 
     BCMOLT_MSG_FIELD_SET(&cfg, state, BCMOLT_FLOW_STATE_ENABLE);
+    BCMOLT_MSG_FIELD_SET(&cfg, statistics, BCMOLT_CONTROL_STATE_ENABLE);
 #ifdef FLOW_CHECKER
     //Flow Checker, To avoid duplicate flow. 
     if (flow_id_counters != 0) {
@@ -1957,7 +2163,7 @@
     return Status::OK;
 }
 
-Status CreateDefaultSchedQueue_(uint32_t intf_id, const std::string direction) {
+bcmos_errno CreateDefaultSched(uint32_t intf_id, const std::string direction) {
     bcmos_errno err;
     bcmolt_tm_sched_cfg tm_sched_cfg;
     bcmolt_tm_sched_key tm_sched_key = {.id = 1};
@@ -2001,33 +2207,14 @@
 
     err = bcmolt_cfg_set(dev_id, &tm_sched_cfg.hdr);
     if (err) {
-        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to create %s scheduler, id %d, intf_id %d, err %d\n", direction.c_str(), tm_sched_key.id, intf_id, err);
-        return Status(grpc::StatusCode::INTERNAL, "Failed to create %s scheduler", direction.c_str());
+        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to create %s scheduler, id %d, intf_id %d, err %d\n", \
+            direction.c_str(), tm_sched_key.id, intf_id, err);
+        return err;
     }
-    OPENOLT_LOG(INFO, openolt_log_id, "Create %s scheduler success, id %d, intf_id %d\n", direction.c_str(), tm_sched_key.id, intf_id);
 
-    // Create 4 Queues for each default PON scheduler
-    for (int queue_id = 0; queue_id < 4; queue_id++) {
-        bcmolt_tm_queue_cfg tm_queue_cfg;
-        bcmolt_tm_queue_key tm_queue_key = {};
-        tm_queue_key.sched_id = get_default_tm_sched_id(intf_id, direction);
-        tm_queue_key.id = queue_id;
-
-        BCMOLT_CFG_INIT(&tm_queue_cfg, tm_queue, tm_queue_key);
-        BCMOLT_MSG_FIELD_SET(&tm_queue_cfg, tm_sched_param.type, BCMOLT_TM_SCHED_PARAM_TYPE_PRIORITY);
-        BCMOLT_MSG_FIELD_SET(&tm_queue_cfg, tm_sched_param.u.priority.priority, queue_id);
-
-        err = bcmolt_cfg_set(dev_id, &tm_queue_cfg.hdr);
-        if (err) {
-            OPENOLT_LOG(ERROR, openolt_log_id, "Failed to create %s tm queue, id %d, sched_id %d\n", \
-                    direction.c_str(), tm_queue_key.id, tm_queue_key.sched_id);
-            return Status(grpc::StatusCode::INTERNAL, "Failed to create %s tm queue", direction.c_str());
-        }
-
-        OPENOLT_LOG(INFO, openolt_log_id, "Create %s tm_queue success, id %d, sched_id %d\n", \
-                direction.c_str(), tm_queue_key.id, tm_queue_key.sched_id);
-    }
-    return Status::OK;
+    OPENOLT_LOG(INFO, openolt_log_id, "Create %s scheduler success, id %d, intf_id %d\n", \
+        direction.c_str(), tm_sched_key.id, intf_id);
+    return BCM_ERR_OK;
 }
 
 bcmos_errno CreateSched(std::string direction, uint32_t intf_id, uint32_t onu_id, uint32_t uni_id, uint32_t port_no,
@@ -2223,15 +2410,11 @@
 
     for (int i = 0; i < traffic_scheds->traffic_scheds_size(); i++) {
         tech_profile::TrafficScheduler traffic_sched = traffic_scheds->traffic_scheds(i);
-        if (traffic_sched.direction() == tech_profile::Direction::UPSTREAM) {
-            direction = upstream;
-        } else if (traffic_sched.direction() == tech_profile::Direction::DOWNSTREAM) {
-            direction = downstream;
-        }
-        else {
-            OPENOLT_LOG(ERROR, openolt_log_id, "direction-not-supported %d\n", traffic_sched.direction());
-            return Status::CANCELLED;
-        }
+
+        direction = GetDirection(traffic_sched.direction());
+        if (direction.compare("direction-not-supported") == 0)
+            return bcm_to_grpc_err(BCM_ERR_PARM, "direction-not-supported");
+
         alloc_id = traffic_sched.alloc_id();
         sched_config = traffic_sched.scheduler();
         additional_bw = sched_config.additional_bw();
@@ -2262,11 +2445,11 @@
         err = bcmolt_cfg_clear(dev_id, &cfg.hdr);
         if (err) {
             OPENOLT_LOG(ERROR, openolt_log_id, "Failed to remove scheduler sched, direction = %s, intf_id %d, alloc_id %d, err %d\n", \
-                    direction.c_str(), intf_id, alloc_id, err);
+                direction.c_str(), intf_id, alloc_id, err);
             return err;
         }
         OPENOLT_LOG(INFO, openolt_log_id, "Removed sched, direction = %s, intf_id %d, alloc_id %d\n", \
-                direction.c_str(), intf_id, alloc_id);
+            direction.c_str(), intf_id, alloc_id);
     } else if (direction == downstream) {
         bcmolt_tm_sched_cfg cfg;
         bcmolt_tm_sched_key key = { };
@@ -2280,12 +2463,12 @@
         BCMOLT_CFG_INIT(&cfg, tm_sched, key);
         err = bcmolt_cfg_clear(dev_id, &(cfg.hdr));
         if (err) {
-            OPENOLT_LOG(ERROR, openolt_log_id, "Failed to remove scheduler sched, direction = %s, id %d, intf_id %d, onu_id %d\n", \
-                    direction.c_str(), key.id, intf_id, onu_id);
+            OPENOLT_LOG(ERROR, openolt_log_id, "Failed to remove scheduler, direction = %s, id %d, intf_id %d, onu_id %d\n", \
+                direction.c_str(), key.id, intf_id, onu_id);
             return err;
         }
         OPENOLT_LOG(INFO, openolt_log_id, "Removed sched, direction = %s, id %d, intf_id %d, onu_id %d\n", \
-                direction.c_str(), key.id, intf_id, onu_id);
+            direction.c_str(), key.id, intf_id, onu_id);
     }
 
     free_tm_sched_id(intf_id, onu_id, uni_id, direction);
@@ -2301,15 +2484,11 @@
 
     for (int i = 0; i < traffic_scheds->traffic_scheds_size(); i++) {
         tech_profile::TrafficScheduler traffic_sched = traffic_scheds->traffic_scheds(i);
-        if (traffic_sched.direction() == tech_profile::Direction::UPSTREAM) {
-            direction = upstream;
-        } else if (traffic_sched.direction() == tech_profile::Direction::DOWNSTREAM) {
-            direction = downstream;
-        }
-        else {
-            OPENOLT_LOG(ERROR, openolt_log_id, "direction-not-supported %d\n", traffic_sched.direction());
-            return Status::CANCELLED;
-        }
+
+        direction = GetDirection(traffic_sched.direction());
+        if (direction.compare("direction-not-supported") == 0)
+            return bcm_to_grpc_err(BCM_ERR_PARM, "direction-not-supported");
+
         int alloc_id = traffic_sched.alloc_id();
         err = RemoveSched(intf_id, onu_id, uni_id, alloc_id, direction);
         if (err) {
@@ -2319,77 +2498,190 @@
     return Status::OK;
 }
 
+bcmos_errno CreateTrafficQueueMappingProfile(uint32_t sched_id, uint32_t intf_id, uint32_t onu_id, uint32_t uni_id, \
+                                             std::string direction, std::vector<uint32_t> tmq_map_profile) {
+    bcmos_errno err;
+    bcmolt_tm_qmp_cfg tm_qmp_cfg;
+    bcmolt_tm_qmp_key tm_qmp_key;
+    bcmolt_arr_u8_8 pbits_to_tmq_id = {0};
+
+    int tm_qmp_id = get_tm_qmp_id(sched_id, intf_id, onu_id, uni_id, tmq_map_profile);
+    if (tm_qmp_id == -1) {
+        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to create tm queue mapping profile. Max allowed profile count is 16.\n");
+    }
+
+    tm_qmp_key.id = tm_qmp_id;
+    for (uint32_t priority=0; priority<tmq_map_profile.size(); priority++) {
+        pbits_to_tmq_id.arr[priority] = tmq_map_profile[priority];
+    }
+
+    BCMOLT_CFG_INIT(&tm_qmp_cfg, tm_qmp, tm_qmp_key);
+    BCMOLT_MSG_FIELD_SET(&tm_qmp_cfg, type, BCMOLT_TM_QMP_TYPE_PBITS);
+    BCMOLT_MSG_FIELD_SET(&tm_qmp_cfg, pbits_to_tmq_id, pbits_to_tmq_id);
+    BCMOLT_MSG_FIELD_SET(&tm_qmp_cfg, ref_count, 0);
+    BCMOLT_MSG_FIELD_SET(&tm_qmp_cfg, state, BCMOLT_CONFIG_STATE_CONFIGURED);
+
+    err = bcmolt_cfg_set(dev_id, &tm_qmp_cfg.hdr);
+    if (err) {
+        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to create tm queue mapping profile, id %d\n", \
+            tm_qmp_key.id);
+        return err;
+    }
+
+    OPENOLT_LOG(INFO, openolt_log_id, "Create tm queue mapping profile success, id %d\n", \
+        tm_qmp_key.id);
+    return BCM_ERR_OK;
+}
+
+bcmos_errno RemoveTrafficQueueMappingProfile(uint32_t tm_qmp_id) {
+    bcmos_errno err;
+    bcmolt_tm_qmp_cfg tm_qmp_cfg;
+    bcmolt_tm_qmp_key tm_qmp_key;
+    tm_qmp_key.id = tm_qmp_id;
+
+    BCMOLT_CFG_INIT(&tm_qmp_cfg, tm_qmp, tm_qmp_key);
+    err = bcmolt_cfg_clear(dev_id, &tm_qmp_cfg.hdr);
+    if (err) {
+        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to remove tm queue mapping profile, id %d\n", \
+            tm_qmp_key.id);
+        return err;
+    }
+
+    OPENOLT_LOG(INFO, openolt_log_id, "Remove tm queue mapping profile success, id %d\n", \
+        tm_qmp_key.id);
+    return BCM_ERR_OK;
+}
+
+bcmos_errno CreateDefaultQueue(uint32_t intf_id, const std::string direction) {
+    bcmos_errno err;
+
+    /* Create 4 Queues on given PON/NNI scheduler */
+    for (int queue_id = 0; queue_id < 4; queue_id++) {
+        bcmolt_tm_queue_cfg tm_queue_cfg;
+        bcmolt_tm_queue_key tm_queue_key = {};
+        tm_queue_key.sched_id = get_default_tm_sched_id(intf_id, direction);
+        tm_queue_key.id = queue_id;
+        if (qos_type == BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE)
+            tm_queue_key.tm_q_set_id = BCMOLT_TM_QUEUE_SET_ID_QSET_NOT_USE;
+        else
+            tm_queue_key.tm_q_set_id = BCMOLT_TM_QUEUE_KEY_TM_Q_SET_ID_DEFAULT;
+
+        BCMOLT_CFG_INIT(&tm_queue_cfg, tm_queue, tm_queue_key);
+        BCMOLT_MSG_FIELD_SET(&tm_queue_cfg, tm_sched_param.type, BCMOLT_TM_SCHED_PARAM_TYPE_PRIORITY);
+        BCMOLT_MSG_FIELD_SET(&tm_queue_cfg, tm_sched_param.u.priority.priority, queue_id);
+
+        err = bcmolt_cfg_set(dev_id, &tm_queue_cfg.hdr);
+        if (err) {
+            OPENOLT_LOG(ERROR, openolt_log_id, "Failed to create %s tm queue, id %d, sched_id %d, tm_q_set_id %d\n", \
+                    direction.c_str(), tm_queue_key.id, tm_queue_key.sched_id, tm_queue_key.tm_q_set_id);
+            return err;
+        }
+
+        OPENOLT_LOG(INFO, openolt_log_id, "Create %s tm_queue success, id %d, sched_id %d, tm_q_set_id %d\n", \
+                direction.c_str(), tm_queue_key.id, tm_queue_key.sched_id, tm_queue_key.tm_q_set_id);
+    }
+    return BCM_ERR_OK;
+}
+
 bcmos_errno CreateQueue(std::string direction, uint32_t access_intf_id, uint32_t onu_id, uint32_t uni_id, uint32_t priority,
                         uint32_t gemport_id) {
     bcmos_errno err;
     bcmolt_tm_queue_cfg cfg;
     bcmolt_tm_queue_key key = { };
-    OPENOLT_LOG(INFO, openolt_log_id, "creating queue. access_intf_id = %d, onu_id = %d, uni_id = %d \
-gemport_id = %d, direction = %s\n", access_intf_id, onu_id, uni_id, gemport_id, direction.c_str());
-    if (direction == downstream) {
-        // There is one queue per gem port
-        key.sched_id = get_tm_sched_id(access_intf_id, onu_id, uni_id, direction);
-        key.id = get_tm_queue_id(access_intf_id, onu_id, uni_id, gemport_id, direction);
+    OPENOLT_LOG(INFO, openolt_log_id, "creating %s queue. access_intf_id = %d, onu_id = %d, uni_id = %d \
+gemport_id = %d\n", direction.c_str(), access_intf_id, onu_id, uni_id, gemport_id);
 
-    } else {
-        queue_map_key_tuple map_key(access_intf_id, onu_id, uni_id, gemport_id, direction);
-        if (queue_map.count(map_key) > 0) {
-            OPENOLT_LOG(INFO, openolt_log_id, "upstream queue exists for intf_id %d, onu_id %d, uni_id %d\n. Not re-creating", \
-                    access_intf_id, onu_id, uni_id); 
-            return BCM_ERR_OK;
-        }
-        key.sched_id = get_default_tm_sched_id(nni_intf_id, direction);
-        if (priority > 7) {
-            return BCM_ERR_RANGE;
-        }
-        // There are 8 queues (one per p-bit)
-        key.id = us_fixed_queue_id_list[priority];
-        update_tm_queue_id(access_intf_id, onu_id, uni_id, gemport_id, direction, key.id);
-        // FIXME: The upstream queues have to be created once only.
-        // The upstream queues on the NNI scheduler are shared by all subscribers.
-        // When the first scheduler comes in, the queues get created, and are re-used by all others.
-        // Also, these queues should be present until the last subscriber exits the system.
-        // One solution is to have these queues always, i.e., create it as soon as OLT is enabled.
+    key.sched_id = (direction.compare(upstream) == 0) ? get_default_tm_sched_id(nni_intf_id, direction) : \
+        get_tm_sched_id(access_intf_id, onu_id, uni_id, direction);
+
+    if (priority > 7) {
+        return BCM_ERR_RANGE;
     }
+
+    /* FIXME: The upstream queues have to be created once only.
+    The upstream queues on the NNI scheduler are shared by all subscribers.
+    When the first scheduler comes in, the queues get created, and are re-used by all others.
+    Also, these queues should be present until the last subscriber exits the system.
+    One solution is to have these queues always, i.e., create it as soon as OLT is enabled.
+
+    There is one queue per gem port and Queue ID is fetched based on priority_q configuration
+    for each GEM in TECH PROFILE */
+    key.id = queue_id_list[priority];
+
     OPENOLT_LOG(INFO, openolt_log_id, "queue assigned queue_id = %d\n", key.id);
 
+    if (qos_type == BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE)
+        key.tm_q_set_id = BCMOLT_TM_QUEUE_SET_ID_QSET_NOT_USE;
+    else if (qos_type == BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE)
+        key.tm_q_set_id = get_tm_qmp_id(key.sched_id, access_intf_id, onu_id, uni_id);
+    else
+        key.tm_q_set_id = BCMOLT_TM_QUEUE_KEY_TM_Q_SET_ID_DEFAULT;
+
     BCMOLT_CFG_INIT(&cfg, tm_queue, key);
     BCMOLT_MSG_FIELD_SET(&cfg, tm_sched_param.u.priority.priority, priority);
 
     err = bcmolt_cfg_set(dev_id, &cfg.hdr);
     if (err) {
-        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to create subscriber tm queue, direction = %s, id %d, sched_id %d, \
-intf_id %d, onu_id %d, uni_id %d, err %d\n", \
-                direction.c_str(), key.id, key.sched_id, access_intf_id, onu_id, uni_id, err);
+        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to create subscriber tm queue, direction = %s, id %d, \
+sched_id %d, tm_q_set_id %d, intf_id %d, onu_id %d, uni_id %d, err %d\n", \
+            direction.c_str(), key.id, key.sched_id, key.tm_q_set_id, access_intf_id, onu_id, uni_id, err);
         return err;
     }
 
-    OPENOLT_LOG(INFO, openolt_log_id, "Created tm_queue, direction %s, id %d, intf_id %d, onu_id %d, uni_id %d\n", \
-            direction.c_str(), key.id, access_intf_id, onu_id, uni_id);
-
+    OPENOLT_LOG(INFO, openolt_log_id, "Created tm_queue, direction %s, id %d, sched_id %d, tm_q_set_id %d, \
+intf_id %d, onu_id %d, uni_id %d\n", direction.c_str(), key.id, key.sched_id, key.tm_q_set_id, access_intf_id, onu_id, uni_id);
     return BCM_ERR_OK;
-
 }
 
 Status CreateTrafficQueues_(const tech_profile::TrafficQueues *traffic_queues) {
     uint32_t intf_id = traffic_queues->intf_id();
     uint32_t onu_id = traffic_queues->onu_id();
     uint32_t uni_id = traffic_queues->uni_id();
+    uint32_t sched_id;
     std::string direction;
     bcmos_errno err;
 
+    qos_type = (traffic_queues->traffic_queues_size() > 1) ? \
+        BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE : BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE;
+
+    if (qos_type == BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE) {
+        uint32_t queues_priority_q[traffic_queues->traffic_queues_size()] = {0};
+        std::string queues_pbit_map[traffic_queues->traffic_queues_size()];
+        for (int i = 0; i < traffic_queues->traffic_queues_size(); i++) {
+            tech_profile::TrafficQueue traffic_queue = traffic_queues->traffic_queues(i);
+
+            direction = GetDirection(traffic_queue.direction());
+            if (direction.compare("direction-not-supported") == 0)
+                return bcm_to_grpc_err(BCM_ERR_PARM, "direction-not-supported");
+
+            queues_priority_q[i] = traffic_queue.priority();
+            queues_pbit_map[i] = traffic_queue.pbit_map();
+        }
+
+        std::vector<uint32_t> tmq_map_profile(8, 0);
+        tmq_map_profile = get_tmq_map_profile(get_valid_queues_pbit_map(queues_pbit_map, COUNT_OF(queues_pbit_map)), \
+                                              queues_priority_q, COUNT_OF(queues_priority_q));
+        sched_id = (direction.compare(upstream) == 0) ? get_default_tm_sched_id(nni_intf_id, direction) : \
+            get_tm_sched_id(intf_id, onu_id, uni_id, direction);
+
+        int tm_qmp_id = get_tm_qmp_id(tmq_map_profile);
+        if (tm_qmp_id == -1) {
+            CreateTrafficQueueMappingProfile(sched_id, intf_id, onu_id, uni_id, direction, tmq_map_profile);
+        } else if (tm_qmp_id != -1 && get_tm_qmp_id(sched_id, intf_id, onu_id, uni_id) == -1) {
+            OPENOLT_LOG(INFO, openolt_log_id, "tm queue mapping profile present already with id %d\n", tm_qmp_id);
+            update_sched_qmp_id_map(sched_id, intf_id, onu_id, uni_id, tm_qmp_id);
+        }
+    }
+
     for (int i = 0; i < traffic_queues->traffic_queues_size(); i++) {
         tech_profile::TrafficQueue traffic_queue = traffic_queues->traffic_queues(i);
-        if (traffic_queue.direction() == tech_profile::Direction::UPSTREAM) {
-            direction = upstream;
-        } else if (traffic_queue.direction() == tech_profile::Direction::DOWNSTREAM) {
-            direction = downstream;
-        }
-        else {
-            OPENOLT_LOG(ERROR, openolt_log_id, "direction-not-supported %d\n", traffic_queue.direction());
-            return Status::CANCELLED;
-        }
+
+        direction = GetDirection(traffic_queue.direction());
+        if (direction.compare("direction-not-supported") == 0)
+            return bcm_to_grpc_err(BCM_ERR_PARM, "direction-not-supported");
+
         err = CreateQueue(direction, intf_id, onu_id, uni_id, traffic_queue.priority(), traffic_queue.gemport_id());
+
         // If the queue exists already, lets not return failure and break the loop.
         if (err && err != BCM_ERR_ALREADY) {
             return bcm_to_grpc_err(err, "Failed to create queue");
@@ -2405,32 +2697,37 @@
     bcmos_errno err;
 
     if (direction == downstream) {
-        if (is_tm_queue_id_present(access_intf_id, onu_id, uni_id, gemport_id, direction) && \
-            is_tm_sched_id_present(access_intf_id, onu_id, uni_id, direction)) {
+        if (is_tm_sched_id_present(access_intf_id, onu_id, uni_id, direction)) {
             key.sched_id = get_tm_sched_id(access_intf_id, onu_id, uni_id, direction);
-            key.id = get_tm_queue_id(access_intf_id, onu_id, uni_id, gemport_id, direction);
+            key.id = queue_id_list[priority];
         } else {
             OPENOLT_LOG(INFO, openolt_log_id, "queue not present in DS. Not clearing, access_intf_id %d, onu_id %d, uni_id %d, gemport_id %d, direction %s\n", access_intf_id, onu_id, uni_id, gemport_id, direction.c_str());
             return BCM_ERR_OK;
         }
     } else {
-        free_tm_queue_id(access_intf_id, onu_id, uni_id, gemport_id, direction);
-        // In the upstream we use pre-created queues on the NNI scheduler that are used by all subscribers.
-        // They should not be removed. So, lets return OK.
+        /* In the upstream we use pre-created queues on the NNI scheduler that are used by all subscribers.
+        They should not be removed. So, lets return OK. */
         return BCM_ERR_OK;
     }
 
+    if (qos_type == BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE)
+        key.tm_q_set_id = BCMOLT_TM_QUEUE_SET_ID_QSET_NOT_USE;
+    else if (qos_type == BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE)
+        key.tm_q_set_id = get_tm_qmp_id(key.sched_id, access_intf_id, onu_id, uni_id);
+    else
+        key.tm_q_set_id = BCMOLT_TM_QUEUE_KEY_TM_Q_SET_ID_DEFAULT;
+
     BCMOLT_CFG_INIT(&cfg, tm_queue, key);
-
     err = bcmolt_cfg_clear(dev_id, &(cfg.hdr));
-
     if (err) {
-        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to remove queue, direction = %s, id %d, sched_id %d, intf_id %d, onu_id %d, uni_id %d\n",
-                direction.c_str(), key.id, key.sched_id, access_intf_id, onu_id, uni_id);
+        OPENOLT_LOG(ERROR, openolt_log_id, "Failed to remove queue, direction = %s, id %d, sched_id %d, \
+tm_q_set_id %d, intf_id %d, onu_id %d, uni_id %d\n",
+                direction.c_str(), key.id, key.sched_id, key.tm_q_set_id, access_intf_id, onu_id, uni_id);
         return err;
     }
 
-    free_tm_queue_id(access_intf_id, onu_id, uni_id, gemport_id, direction);
+    OPENOLT_LOG(INFO, openolt_log_id, "Removed tm_queue, direction %s, id %d, sched_id %d, tm_q_set_id %d, \
+intf_id %d, onu_id %d, uni_id %d\n", direction.c_str(), key.id, key.sched_id, key.tm_q_set_id, access_intf_id, onu_id, uni_id);
 
     return BCM_ERR_OK;
 }
@@ -2440,24 +2737,34 @@
     uint32_t onu_id = traffic_queues->onu_id();
     uint32_t uni_id = traffic_queues->uni_id();
     uint32_t port_no = traffic_queues->port_no();
+    uint32_t sched_id;
     std::string direction;
     bcmos_errno err;
 
+    qos_type = (traffic_queues->traffic_queues_size() > 1) ? \
+        BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE : BCMOLT_EGRESS_QOS_TYPE_FIXED_QUEUE;
+
     for (int i = 0; i < traffic_queues->traffic_queues_size(); i++) {
         tech_profile::TrafficQueue traffic_queue = traffic_queues->traffic_queues(i);
-        if (traffic_queue.direction() == tech_profile::Direction::UPSTREAM) {
-            direction = upstream;
-        } else if (traffic_queue.direction() == tech_profile::Direction::DOWNSTREAM) {
-            direction = downstream;
-        } else {
-            OPENOLT_LOG(ERROR, openolt_log_id, "direction-not-supported %d\n", traffic_queue.direction());
-            return Status::CANCELLED;
-        }
+
+        direction = GetDirection(traffic_queue.direction());
+        if (direction.compare("direction-not-supported") == 0)
+            return bcm_to_grpc_err(BCM_ERR_PARM, "direction-not-supported");
+
         err = RemoveQueue(direction, intf_id, onu_id, uni_id, traffic_queue.priority(), traffic_queue.gemport_id());
         if (err) {
             return bcm_to_grpc_err(err, "Failed to remove queue");
         }
     }
 
+    if (qos_type == BCMOLT_EGRESS_QOS_TYPE_PRIORITY_TO_QUEUE && (direction.compare(upstream) == 0 || direction.compare(downstream) == 0 && is_tm_sched_id_present(intf_id, onu_id, uni_id, direction))) {
+        sched_id = (direction.compare(upstream) == 0) ? get_default_tm_sched_id(nni_intf_id, direction) : \
+            get_tm_sched_id(intf_id, onu_id, uni_id, direction);
+
+        int tm_qmp_id = get_tm_qmp_id(sched_id, intf_id, onu_id, uni_id);
+        if (free_tm_qmp_id(sched_id, intf_id, onu_id, uni_id, tm_qmp_id)) {
+            RemoveTrafficQueueMappingProfile(tm_qmp_id);
+        }
+    }
     return Status::OK;
 }