Snap for 8580505 from fd6e7cbcf0e007fa6deb01e67d67c78511478cfa to main-cg-testing-release

Change-Id: If424bb6f2970dd477d588fb28a8eb9e927762750
diff --git a/README.md b/README.md
index 74b9376..60daa85 100644
--- a/README.md
+++ b/README.md
@@ -30,7 +30,8 @@
   libprotobuf-dev ninja-build generate-ninja protobuf-compiler \
   libre2-9 debmake \
   llvm libc++abi-dev \
-  libre2-dev libdouble-conversion-dev
+  libre2-dev libdouble-conversion-dev \
+  libgtest-dev libgmock-dev libabsl-dev
 ```
 
 You will also need a recent-ish version of Rust and Cargo. Please follow the
@@ -89,26 +90,6 @@
 sudo dpkg -i outdir/libchrome/*.deb
 ```
 
-The googletest packages provided by Debian/Ubuntu (libgmock-dev and
-libgtest-dev) do not provide pkg-config files, so you can build your own
-googletest using the steps below:
-
-```sh
-git clone https://github.com/google/googletest.git -b release-1.10.0
-cd googletest        # Main directory of the cloned repository.
-mkdir build          # Create a directory to hold the build output.
-cd build
-cmake ..             # Generate native build scripts for GoogleTest.
-sudo make install -DCMAKE_INSTALL_PREFIX=/usr
-
-# Optional steps if pkgconfig isn't installed to desired location
-# Modify the source (/usr/lib/x86_64-linux-gnu) and target (/usr/lib) based on
-# your local installation.
-for f in $(ls /usr/lib/x86_64-linux-gnu/pkgconfig/{gtest,gmock}*); do \
-  ln -sf $f /usr/lib/pkgconfig/$(basename $f);
-done
-```
-
 ### Rust dependencies
 
 **Note**: Handled by `--run-bootstrap` option.
diff --git a/android/app/jni/com_android_bluetooth_vc.cpp b/android/app/jni/com_android_bluetooth_vc.cpp
index 22baaea..b479da6 100644
--- a/android/app/jni/com_android_bluetooth_vc.cpp
+++ b/android/app/jni/com_android_bluetooth_vc.cpp
@@ -359,6 +359,60 @@
   sVolumeControlInterface->SetVolume(group_id, volume);
 }
 
+static void muteNative(JNIEnv* env, jobject object, jbyteArray address) {
+  if (!sVolumeControlInterface) {
+    LOG(ERROR) << __func__
+               << ": Failed to get the Bluetooth Volume Control Interface";
+    return;
+  }
+
+  jbyte* addr = env->GetByteArrayElements(address, nullptr);
+  if (!addr) {
+    jniThrowIOException(env, EINVAL);
+    return;
+  }
+
+  RawAddress* tmpraw = (RawAddress*)addr;
+  sVolumeControlInterface->Mute(*tmpraw);
+  env->ReleaseByteArrayElements(address, addr, 0);
+}
+
+static void muteGroupNative(JNIEnv* env, jobject object, jint group_id) {
+  if (!sVolumeControlInterface) {
+    LOG(ERROR) << __func__
+               << ": Failed to get the Bluetooth Volume Control Interface";
+    return;
+  }
+  sVolumeControlInterface->Mute(group_id);
+}
+
+static void unmuteNative(JNIEnv* env, jobject object, jbyteArray address) {
+  if (!sVolumeControlInterface) {
+    LOG(ERROR) << __func__
+               << ": Failed to get the Bluetooth Volume Control Interface";
+    return;
+  }
+
+  jbyte* addr = env->GetByteArrayElements(address, nullptr);
+  if (!addr) {
+    jniThrowIOException(env, EINVAL);
+    return;
+  }
+
+  RawAddress* tmpraw = (RawAddress*)addr;
+  sVolumeControlInterface->Unmute(*tmpraw);
+  env->ReleaseByteArrayElements(address, addr, 0);
+}
+
+static void unmuteGroupNative(JNIEnv* env, jobject object, jint group_id) {
+  if (!sVolumeControlInterface) {
+    LOG(ERROR) << __func__
+               << ": Failed to get the Bluetooth Volume Control Interface";
+    return;
+  }
+  sVolumeControlInterface->Unmute(group_id);
+}
+
 /* Native methods for exterbak audio outputs */
 static jboolean getExtAudioOutVolumeOffsetNative(JNIEnv* env, jobject object,
                                                  jbyteArray address,
@@ -496,6 +550,10 @@
      (void*)disconnectVolumeControlNative},
     {"setVolumeNative", "([BI)V", (void*)setVolumeNative},
     {"setVolumeGroupNative", "(II)V", (void*)setVolumeGroupNative},
+    {"muteNative", "([B)V", (void*)muteNative},
+    {"muteGroupNative", "(I)V", (void*)muteGroupNative},
+    {"unmuteNative", "([B)V", (void*)unmuteNative},
+    {"unmuteGroupNative", "(I)V", (void*)unmuteGroupNative},
     {"getExtAudioOutVolumeOffsetNative", "([BI)Z",
      (void*)getExtAudioOutVolumeOffsetNative},
     {"setExtAudioOutVolumeOffsetNative", "([BII)Z",
diff --git a/android/app/src/com/android/bluetooth/bass_client/BassClientService.java b/android/app/src/com/android/bluetooth/bass_client/BassClientService.java
index 497a3ad..c2e9f4c 100755
--- a/android/app/src/com/android/bluetooth/bass_client/BassClientService.java
+++ b/android/app/src/com/android/bluetooth/bass_client/BassClientService.java
@@ -83,7 +83,6 @@
     private Map<BluetoothDevice, Integer> mDeviceToSyncHandleMap;
     /*syncHandle, parsed BaseData data*/
     private Map<Integer, BaseData> mSyncHandleToBaseDataMap;
-    private Map<Integer, BluetoothLeBroadcastMetadata> mBroadcastSources;
     /*bcastSrcDevice, corresponding PeriodicAdvertisementResult*/
     private Map<BluetoothDevice, PeriodicAdvertisementResult> mPeriodicAdvertisementResultMap;
     private ScanCallback mSearchScanCallback;
@@ -181,25 +180,6 @@
         return base;
     }
 
-    void updateSourceInternal(int sourceId, BluetoothLeBroadcastMetadata metaData) {
-        if (mBroadcastSources == null) {
-            return;
-        }
-        if (metaData != null) {
-            // This will replace old metadata with new one
-            mBroadcastSources.put(sourceId, metaData);
-        } else {
-            mBroadcastSources.remove(sourceId);
-        }
-    }
-
-    BluetoothLeBroadcastMetadata getSourceInternal(int sourceId) {
-        if (mBroadcastSources != null) {
-            return mBroadcastSources.get(sourceId);
-        }
-        return null;
-    }
-
     void setActiveSyncedSource(BluetoothDevice scanDelegator, BluetoothDevice sourceDevice) {
         log("setActiveSyncedSource: scanDelegator" + scanDelegator
                 + ":: sourceDevice:" + sourceDevice);
@@ -283,10 +263,6 @@
             mActiveSourceMap.clear();
             mActiveSourceMap = null;
         }
-        if (mBroadcastSources != null) {
-            mBroadcastSources.clear();
-            mBroadcastSources = null;
-        }
         if (mBassUtils != null) {
             mBassUtils.cleanUp();
             mBassUtils = null;
diff --git a/android/app/src/com/android/bluetooth/bass_client/BassClientStateMachine.java b/android/app/src/com/android/bluetooth/bass_client/BassClientStateMachine.java
index ab56843..aa6d15a 100755
--- a/android/app/src/com/android/bluetooth/bass_client/BassClientStateMachine.java
+++ b/android/app/src/com/android/bluetooth/bass_client/BassClientStateMachine.java
@@ -56,6 +56,8 @@
  */
 package com.android.bluetooth.bass_client;
 
+import static android.Manifest.permission.BLUETOOTH_CONNECT;
+
 import android.bluetooth.BluetoothAdapter;
 import android.bluetooth.BluetoothDevice;
 import android.bluetooth.BluetoothGatt;
@@ -65,6 +67,7 @@
 import android.bluetooth.BluetoothGattService;
 import android.bluetooth.BluetoothLeAudioCodecConfigMetadata;
 import android.bluetooth.BluetoothLeAudioContentMetadata;
+import android.bluetooth.BluetoothLeBroadcastAssistant;
 import android.bluetooth.BluetoothLeBroadcastChannel;
 import android.bluetooth.BluetoothLeBroadcastMetadata;
 import android.bluetooth.BluetoothLeBroadcastReceiveState;
@@ -76,6 +79,7 @@
 import android.bluetooth.le.PeriodicAdvertisingReport;
 import android.bluetooth.le.ScanRecord;
 import android.bluetooth.le.ScanResult;
+import android.content.Intent;
 import android.os.Binder;
 import android.os.Looper;
 import android.os.Message;
@@ -83,6 +87,7 @@
 import android.provider.DeviceConfig;
 import android.util.Log;
 
+import com.android.bluetooth.Utils;
 import com.android.bluetooth.btservice.ProfileService;
 import com.android.bluetooth.btservice.ServiceFactory;
 import com.android.internal.annotations.VisibleForTesting;
@@ -136,6 +141,7 @@
     private final Map<Integer, BluetoothLeBroadcastReceiveState>
             mBluetoothLeBroadcastReceiveStates =
             new HashMap<Integer, BluetoothLeBroadcastReceiveState>();
+    private final Map<Integer, BluetoothLeBroadcastMetadata> mCurrentMetadata = new HashMap();
     private final Disconnected mDisconnected = new Disconnected();
     private final Connected mConnected = new Connected();
     private final Connecting mConnecting = new Connecting();
@@ -236,6 +242,20 @@
         mPendingOperation = -1;
         mPendingSourceId = -1;
         mPendingMetadata = null;
+        mCurrentMetadata.clear();
+    }
+
+    BluetoothLeBroadcastMetadata getCurrentBroadcastMetadata(Integer sourceId) {
+        return mCurrentMetadata.getOrDefault(sourceId, null);
+    }
+
+    private void setCurrentBroadcastMetadata(Integer sourceId,
+            BluetoothLeBroadcastMetadata metadata) {
+        if (metadata != null) {
+            mCurrentMetadata.put(sourceId, metadata);
+        } else {
+            mCurrentMetadata.remove(sourceId);
+        }
     }
 
     BluetoothLeBroadcastReceiveState getBroadcastReceiveStateForSourceDevice(
@@ -720,7 +740,7 @@
                 mService.getCallbacks().notifySourceAdded(mDevice,
                         recvState.getSourceId(), BluetoothStatusCodes.REASON_LOCAL_APP_REQUEST);
                 if (mPendingMetadata != null) {
-                    mService.updateSourceInternal(recvState.getSourceId(), mPendingMetadata);
+                    setCurrentBroadcastMetadata(recvState.getSourceId(), mPendingMetadata);
                 }
                 checkAndUpdateBroadcastCode(recvState);
                 processPASyncState(recvState);
@@ -730,13 +750,13 @@
                     BluetoothDevice removedDevice = oldRecvState.getSourceDevice();
                     log("sourceInfo removal" + removedDevice);
                     cancelActiveSync(removedDevice);
-                    mService.updateSourceInternal(oldRecvState.getSourceId(), null);
+                    setCurrentBroadcastMetadata(oldRecvState.getSourceId(), null);
                     mService.getCallbacks().notifySourceRemoved(mDevice,
                             oldRecvState.getSourceId(),
                             BluetoothStatusCodes.REASON_LOCAL_APP_REQUEST);
                 } else {
                     log("update to an existing recvState");
-                    mService.updateSourceInternal(recvState.getSourceId(), mPendingMetadata);
+                    setCurrentBroadcastMetadata(recvState.getSourceId(), mPendingMetadata);
                     mService.getCallbacks().notifySourceModified(mDevice,
                             recvState.getSourceId(), BluetoothStatusCodes.REASON_LOCAL_APP_REQUEST);
                     checkAndUpdateBroadcastCode(recvState);
@@ -955,6 +975,7 @@
         }
         mPendingOperation = -1;
         mPendingMetadata = null;
+        mCurrentMetadata.clear();
     }
 
     @VisibleForTesting
@@ -1285,8 +1306,8 @@
         res[1] = (byte) recvState.getSourceId();
         log("convertRecvStateToSetBroadcastCodeByteArray: Source device : "
                 + recvState.getSourceDevice());
-        BluetoothLeBroadcastMetadata metaData = mService.getSourceInternal(
-                recvState.getSourceId());
+        BluetoothLeBroadcastMetadata metaData =
+                getCurrentBroadcastMetadata(recvState.getSourceId());
         if (metaData == null) {
             Log.e(TAG, "Fail to find broadcast source, sourceId = "
                     + recvState.getSourceId());
@@ -1346,7 +1367,7 @@
                     + messageWhatToString(getCurrentMessage().what));
             removeDeferredMessages(CONNECT);
             if (mLastConnectionState == BluetoothProfile.STATE_CONNECTED) {
-                log("CONNECTED->CONNTECTED: Ignore");
+                log("CONNECTED->CONNECTED: Ignore");
             } else {
                 broadcastConnectionState(mDevice, mLastConnectionState,
                         BluetoothProfile.STATE_CONNECTED);
@@ -1743,9 +1764,17 @@
         log("broadcastConnectionState " + device + ": " + fromState + "->" + toState);
         if (fromState == BluetoothProfile.STATE_CONNECTED
                 && toState == BluetoothProfile.STATE_CONNECTED) {
-            log("CONNECTED->CONNTECTED: Ignore");
+            log("CONNECTED->CONNECTED: Ignore");
             return;
         }
+
+        Intent intent = new Intent(BluetoothLeBroadcastAssistant.ACTION_CONNECTION_STATE_CHANGED);
+        intent.putExtra(BluetoothProfile.EXTRA_PREVIOUS_STATE, fromState);
+        intent.putExtra(BluetoothProfile.EXTRA_STATE, toState);
+        intent.putExtra(BluetoothDevice.EXTRA_DEVICE, mDevice);
+        intent.addFlags(Intent.FLAG_RECEIVER_REGISTERED_ONLY_BEFORE_BOOT
+                    | Intent.FLAG_RECEIVER_INCLUDE_BACKGROUND);
+        mService.sendBroadcast(intent, BLUETOOTH_CONNECT, Utils.getTempAllowlistBroadcastOptions());
     }
 
     int getConnectionState() {
diff --git a/android/app/src/com/android/bluetooth/btservice/ProfileService.java b/android/app/src/com/android/bluetooth/btservice/ProfileService.java
index 475b1e5..7178ae3 100644
--- a/android/app/src/com/android/bluetooth/btservice/ProfileService.java
+++ b/android/app/src/com/android/bluetooth/btservice/ProfileService.java
@@ -256,7 +256,7 @@
                 component,
                 enable ? PackageManager.COMPONENT_ENABLED_STATE_ENABLED
                        : PackageManager.COMPONENT_ENABLED_STATE_DISABLED,
-                PackageManager.DONT_KILL_APP);
+                PackageManager.DONT_KILL_APP | PackageManager.SYNCHRONOUS);
     }
 
     /**
diff --git a/android/app/src/com/android/bluetooth/le_audio/LeAudioService.java b/android/app/src/com/android/bluetooth/le_audio/LeAudioService.java
index 612d5f4..7884604 100644
--- a/android/app/src/com/android/bluetooth/le_audio/LeAudioService.java
+++ b/android/app/src/com/android/bluetooth/le_audio/LeAudioService.java
@@ -175,6 +175,8 @@
 
     private BroadcastReceiver mBondStateChangedReceiver;
     private BroadcastReceiver mConnectionStateChangedReceiver;
+    private BroadcastReceiver mMuteStateChangedReceiver;
+    private int mStoredRingerMode = -1;
     private Handler mHandler = new Handler(Looper.getMainLooper());
 
     private final Map<Integer, Integer> mBroadcastStateMap = new HashMap<>();
@@ -238,6 +240,11 @@
         filter.addAction(BluetoothLeAudio.ACTION_LE_AUDIO_CONNECTION_STATE_CHANGED);
         mConnectionStateChangedReceiver = new ConnectionStateChangedReceiver();
         registerReceiver(mConnectionStateChangedReceiver, filter);
+        filter = new IntentFilter();
+        filter.addAction(AudioManager.RINGER_MODE_CHANGED_ACTION);
+        mMuteStateChangedReceiver = new MuteStateChangedReceiver();
+        registerReceiver(mMuteStateChangedReceiver, filter);
+
         mLeAudioCallbacks = new RemoteCallbackList<IBluetoothLeAudioCallback>();
 
         int tmapRoleMask =
@@ -329,6 +336,8 @@
         mBondStateChangedReceiver = null;
         unregisterReceiver(mConnectionStateChangedReceiver);
         mConnectionStateChangedReceiver = null;
+        unregisterReceiver(mMuteStateChangedReceiver);
+        mMuteStateChangedReceiver = null;
 
         // Destroy state machines and stop handler thread
         synchronized (mStateMachines) {
@@ -1532,6 +1541,39 @@
         }
     }
 
+    private synchronized boolean isSilentModeEnabled() {
+        return mStoredRingerMode != AudioManager.RINGER_MODE_NORMAL;
+    }
+
+    private class MuteStateChangedReceiver extends BroadcastReceiver {
+        @Override
+        public void onReceive(Context context, Intent intent) {
+            if (!AudioManager.RINGER_MODE_CHANGED_ACTION.equals(intent.getAction())) {
+                return;
+            }
+
+            final String action = intent.getAction();
+            if (action.equals(AudioManager.RINGER_MODE_CHANGED_ACTION)) {
+                int ringerMode = intent.getIntExtra(AudioManager.EXTRA_RINGER_MODE, -1);
+
+                if (ringerMode < 0 || ringerMode == mStoredRingerMode) return;
+
+                mStoredRingerMode = ringerMode;
+                int activeGroupId = getActiveGroupId();
+                if (activeGroupId == LE_AUDIO_GROUP_ID_INVALID) return;
+
+                VolumeControlService service = mServiceFactory.getVolumeControlService();
+                if (service == null) return;
+
+                if (isSilentModeEnabled()) {
+                    service.muteGroup(activeGroupId);
+                } else {
+                    service.unmuteGroup(activeGroupId);
+                }
+            }
+        }
+    }
+
    /**
      * Check whether can connect to a peer device.
      * The check considers a number of factors during the evaluation.
diff --git a/android/app/src/com/android/bluetooth/pbap/BluetoothPbapObexServer.java b/android/app/src/com/android/bluetooth/pbap/BluetoothPbapObexServer.java
index de2d586..403bb71 100644
--- a/android/app/src/com/android/bluetooth/pbap/BluetoothPbapObexServer.java
+++ b/android/app/src/com/android/bluetooth/pbap/BluetoothPbapObexServer.java
@@ -101,6 +101,17 @@
             "/telecom/och",
             "/telecom/mch",
             "/telecom/cch",
+    };
+
+    // Currently not support SIM card
+    @SuppressWarnings("unused") private static final String[] LEGAL_PATH_WITH_SIM = {
+            "/telecom",
+            "/telecom/pb",
+            "/telecom/fav",
+            "/telecom/ich",
+            "/telecom/och",
+            "/telecom/mch",
+            "/telecom/cch",
             "/SIM1",
             "/SIM1/telecom",
             "/SIM1/telecom/ich",
@@ -108,7 +119,6 @@
             "/SIM1/telecom/mch",
             "/SIM1/telecom/cch",
             "/SIM1/telecom/pb"
-
     };
 
     // SIM card
@@ -463,23 +473,21 @@
                 appParamValue.needTag = ContentType.PHONEBOOK;
             } else if (mCurrentPath.equals(FAV_PATH)) {
                 appParamValue.needTag = ContentType.FAVORITES;
-            } else if (mCurrentPath.equals(ICH_PATH) || mCurrentPath.equals(SIM_ICH_PATH)) {
+            } else if (mCurrentPath.equals(ICH_PATH)) {
                 appParamValue.needTag = ContentType.INCOMING_CALL_HISTORY;
-            } else if (mCurrentPath.equals(OCH_PATH)|| mCurrentPath.equals(SIM_OCH_PATH)) {
+            } else if (mCurrentPath.equals(OCH_PATH)) {
                 appParamValue.needTag = ContentType.OUTGOING_CALL_HISTORY;
-            } else if (mCurrentPath.equals(MCH_PATH)|| mCurrentPath.equals(SIM_MCH_PATH)) {
+            } else if (mCurrentPath.equals(MCH_PATH)) {
                 appParamValue.needTag = ContentType.MISSED_CALL_HISTORY;
                 mNeedNewMissedCallsNum = true;
-            } else if (mCurrentPath.equals(CCH_PATH)|| mCurrentPath.equals(SIM_CCH_PATH)) {
+            } else if (mCurrentPath.equals(CCH_PATH)) {
                 appParamValue.needTag = ContentType.COMBINED_CALL_HISTORY;
-            } else if (mCurrentPath.equals(TELECOM_PATH)|| mCurrentPath.equals(SIM_PATH)) {
+            } else if (mCurrentPath.equals(TELECOM_PATH)) {
                 /* PBAP 1.1.1 change */
                 if (!validName && type.equals(TYPE_LISTING)) {
                     Log.e(TAG, "invalid vcard listing request in default folder");
                     return ResponseCodes.OBEX_HTTP_NOT_FOUND;
                 }
-            } else if (mCurrentPath.equals(SIM_PB_PATH)) {
-                appParamValue.needTag = ContentType.SIM_PHONEBOOK;
             } else {
                 Log.w(TAG, "mCurrentpath is not valid path!!!");
                 return ResponseCodes.OBEX_HTTP_NOT_ACCEPTABLE;
@@ -495,6 +503,9 @@
                 TYPE_PB, TYPE_LISTING, mCurrentPath)) {
                 appParamValue.needTag = ContentType.SIM_PHONEBOOK;
                 if (D) Log.d(TAG, "download SIM phonebook request");
+                // Not support SIM card currently
+                Log.w(TAG, "Not support access SIM card info!");
+                return ResponseCodes.OBEX_HTTP_NOT_ACCEPTABLE;
             } else if (isNameMatchTarget(name, PB)) {
                 appParamValue.needTag = ContentType.PHONEBOOK;
                 if (D) {
diff --git a/android/app/src/com/android/bluetooth/pbap/BluetoothPbapService.java b/android/app/src/com/android/bluetooth/pbap/BluetoothPbapService.java
index ba80fe3..3b81bd5 100644
--- a/android/app/src/com/android/bluetooth/pbap/BluetoothPbapService.java
+++ b/android/app/src/com/android/bluetooth/pbap/BluetoothPbapService.java
@@ -154,7 +154,7 @@
 
     private static final int SDP_PBAP_SERVER_VERSION = 0x0102;
     // PBAP v1.2.3, Sec. 7.1.2: local phonebook and favorites
-    private static final int SDP_PBAP_SUPPORTED_REPOSITORIES = 0x000B;
+    private static final int SDP_PBAP_SUPPORTED_REPOSITORIES = 0x0009;
     private static final int SDP_PBAP_SUPPORTED_FEATURES = 0x021F;
 
     /* PBAP will use Bluetooth notification ID from 1000000 (included) to 2000000 (excluded).
diff --git a/android/app/src/com/android/bluetooth/tbs/TbsGeneric.java b/android/app/src/com/android/bluetooth/tbs/TbsGeneric.java
index 704f55a..2a281ab 100644
--- a/android/app/src/com/android/bluetooth/tbs/TbsGeneric.java
+++ b/android/app/src/com/android/bluetooth/tbs/TbsGeneric.java
@@ -181,6 +181,9 @@
             mTbsGatt.clearSilentModeFlag();
         }
 
+        // Android supports inband ringtone
+        mTbsGatt.setInbandRingtoneFlag();
+
         mReceiver = new Receiver();
         mTbsGatt.getContext().registerReceiver(mReceiver,
                 new IntentFilter(AudioManager.RINGER_MODE_CHANGED_ACTION));
diff --git a/android/app/src/com/android/bluetooth/vc/VolumeControlNativeInterface.java b/android/app/src/com/android/bluetooth/vc/VolumeControlNativeInterface.java
index 421405c..0a50c6d 100644
--- a/android/app/src/com/android/bluetooth/vc/VolumeControlNativeInterface.java
+++ b/android/app/src/com/android/bluetooth/vc/VolumeControlNativeInterface.java
@@ -117,6 +117,44 @@
         setVolumeGroupNative(groupId, volume);
     }
 
+     /**
+     * Mute the VolumeControl volume
+     * @param device
+     * @param unmute
+     */
+    @VisibleForTesting(visibility = VisibleForTesting.Visibility.PACKAGE)
+    public void mute(BluetoothDevice device) {
+        muteNative(getByteAddress(device));
+    }
+
+    /**
+     * Mute the VolumeControl volume in the group
+     * @param groupId
+     * @param unmute
+     */
+    @VisibleForTesting(visibility = VisibleForTesting.Visibility.PACKAGE)
+    public void muteGroup(int groupId) {
+        muteGroupNative(groupId);
+    }
+
+    /**
+     * Unmute the VolumeControl volume
+     */
+    @VisibleForTesting(visibility = VisibleForTesting.Visibility.PACKAGE)
+    public void unmute(BluetoothDevice device) {
+        unmuteNative(getByteAddress(device));
+    }
+
+     /**
+     * Unmute the VolumeControl volume group
+     * @param groupId
+     * @param unmute
+     */
+    @VisibleForTesting(visibility = VisibleForTesting.Visibility.PACKAGE)
+    public void unmuteGroup(int groupId) {
+        unmuteGroupNative(groupId);
+    }
+
     /**
      * Gets external audio output volume offset from a remote device.
      *
@@ -333,6 +371,10 @@
     private native boolean disconnectVolumeControlNative(byte[] address);
     private native void setVolumeNative(byte[] address, int volume);
     private native void setVolumeGroupNative(int groupId, int volume);
+    private native void muteNative(byte[] address);
+    private native void muteGroupNative(int groupId);
+    private native void unmuteNative(byte[] address);
+    private native void unmuteGroupNative(int groupId);
     private native boolean getExtAudioOutVolumeOffsetNative(byte[] address, int externalOutputId);
     private native boolean setExtAudioOutVolumeOffsetNative(byte[] address, int externalOutputId,
                                                                 int offset);
diff --git a/android/app/src/com/android/bluetooth/vc/VolumeControlService.java b/android/app/src/com/android/bluetooth/vc/VolumeControlService.java
index 477b6f2..825a8cf 100644
--- a/android/app/src/com/android/bluetooth/vc/VolumeControlService.java
+++ b/android/app/src/com/android/bluetooth/vc/VolumeControlService.java
@@ -582,6 +582,34 @@
         mVolumeControlNativeInterface.setVolumeGroup(groupId, volume);
     }
 
+    /**
+     * {@hide}
+     */
+    public void mute(BluetoothDevice device) {
+        mVolumeControlNativeInterface.mute(device);
+    }
+
+    /**
+     * {@hide}
+     */
+    public void muteGroup(int groupId) {
+        mVolumeControlNativeInterface.muteGroup(groupId);
+    }
+
+    /**
+     * {@hide}
+     */
+    public void unmute(BluetoothDevice device) {
+        mVolumeControlNativeInterface.unmute(device);
+    }
+
+    /**
+     * {@hide}
+     */
+    public void unmuteGroup(int groupId) {
+        mVolumeControlNativeInterface.unmuteGroup(groupId);
+    }
+
     void handleVolumeControlChanged(BluetoothDevice device, int groupId,
                                     int volume, boolean mute, boolean isAutonomous) {
         if (!isAutonomous) {
@@ -1114,6 +1142,76 @@
         }
 
         @Override
+        public void mute(BluetoothDevice device,  AttributionSource source,
+                SynchronousResultReceiver receiver) {
+            try {
+                Objects.requireNonNull(device, "device cannot be null");
+                Objects.requireNonNull(source, "source cannot be null");
+                Objects.requireNonNull(receiver, "receiver cannot be null");
+
+                VolumeControlService service = getService(source);
+                if (service != null) {
+                    service.mute(device);
+                }
+                receiver.send(null);
+            } catch (RuntimeException e) {
+                receiver.propagateException(e);
+            }
+        }
+
+        @Override
+        public void muteGroup(int groupId, AttributionSource source,
+                SynchronousResultReceiver receiver) {
+            try {
+                Objects.requireNonNull(source, "source cannot be null");
+                Objects.requireNonNull(receiver, "receiver cannot be null");
+
+                VolumeControlService service = getService(source);
+                if (service != null) {
+                    service.muteGroup(groupId);
+                }
+                receiver.send(null);
+            } catch (RuntimeException e) {
+                receiver.propagateException(e);
+            }
+        }
+
+        @Override
+        public void unmute(BluetoothDevice device,  AttributionSource source,
+                SynchronousResultReceiver receiver) {
+            try {
+                Objects.requireNonNull(device, "device cannot be null");
+                Objects.requireNonNull(source, "source cannot be null");
+                Objects.requireNonNull(receiver, "receiver cannot be null");
+
+                VolumeControlService service = getService(source);
+                if (service != null) {
+                    service.unmute(device);
+                }
+                receiver.send(null);
+            } catch (RuntimeException e) {
+                receiver.propagateException(e);
+            }
+        }
+
+        @Override
+        public void unmuteGroup(int groupId,  AttributionSource source,
+                SynchronousResultReceiver receiver) {
+            try {
+                Objects.requireNonNull(source, "source cannot be null");
+                Objects.requireNonNull(receiver, "receiver cannot be null");
+
+                VolumeControlService service = getService(source);
+                if (service != null) {
+                    service.unmuteGroup(groupId);
+                }
+                receiver.send(null);
+            } catch (RuntimeException e) {
+                receiver.propagateException(e);
+            }
+        }
+
+        @Override
         public void registerCallback(IBluetoothVolumeControlCallback callback,
                 AttributionSource source, SynchronousResultReceiver receiver) {
             try {
diff --git a/system/audio_hal_interface/aidl/le_audio_software_aidl.cc b/system/audio_hal_interface/aidl/le_audio_software_aidl.cc
index ee8969b..40f991e 100644
--- a/system/audio_hal_interface/aidl/le_audio_software_aidl.cc
+++ b/system/audio_hal_interface/aidl/le_audio_software_aidl.cc
@@ -360,6 +360,7 @@
     {30, ::le_audio::codec_spec_conf::kLeAudioCodecLC3FrameLen30},
     {40, ::le_audio::codec_spec_conf::kLeAudioCodecLC3FrameLen40},
     {60, ::le_audio::codec_spec_conf::kLeAudioCodecLC3FrameLen60},
+    {80, ::le_audio::codec_spec_conf::kLeAudioCodecLC3FrameLen80},
     {120, ::le_audio::codec_spec_conf::kLeAudioCodecLC3FrameLen120}};
 
 std::unordered_map<AudioLocation, uint32_t> audio_location_map{
diff --git a/system/binder/android/bluetooth/IBluetoothVolumeControl.aidl b/system/binder/android/bluetooth/IBluetoothVolumeControl.aidl
index 9049260..eb6ddd1 100644
--- a/system/binder/android/bluetooth/IBluetoothVolumeControl.aidl
+++ b/system/binder/android/bluetooth/IBluetoothVolumeControl.aidl
@@ -52,6 +52,16 @@
     @JavaPassthrough(annotation="@android.annotation.RequiresPermission(android.Manifest.permission.BLUETOOTH_CONNECT)")
     void setVolumeGroup(int group_id, int volume, in AttributionSource attributionSource, in SynchronousResultReceiver receiver);
 
+    @JavaPassthrough(annotation="@android.annotation.RequiresPermission(android.Manifest.permission.BLUETOOTH_CONNECT)")
+    void mute(in BluetoothDevice device, in AttributionSource attributionSource, in SynchronousResultReceiver receiver);
+    @JavaPassthrough(annotation="@android.annotation.RequiresPermission(android.Manifest.permission.BLUETOOTH_CONNECT)")
+    void muteGroup(int group_id, in AttributionSource attributionSource, in SynchronousResultReceiver receiver);
+
+    @JavaPassthrough(annotation="@android.annotation.RequiresPermission(android.Manifest.permission.BLUETOOTH_CONNECT)")
+    void unmute(in BluetoothDevice device, in AttributionSource attributionSource, in SynchronousResultReceiver receiver);
+    @JavaPassthrough(annotation="@android.annotation.RequiresPermission(android.Manifest.permission.BLUETOOTH_CONNECT)")
+    void unmuteGroup(int group_id, in AttributionSource attributionSource, in SynchronousResultReceiver receiver);
+
     @JavaPassthrough(annotation="@android.annotation.RequiresPermission(allOf={android.Manifest.permission.BLUETOOTH_CONNECT,android.Manifest.permission.BLUETOOTH_PRIVILEGED})")
     void registerCallback(in IBluetoothVolumeControlCallback callback, in AttributionSource attributionSource, in SynchronousResultReceiver receiver);
     @JavaPassthrough(annotation="@android.annotation.RequiresPermission(allOf={android.Manifest.permission.BLUETOOTH_CONNECT,android.Manifest.permission.BLUETOOTH_PRIVILEGED})")
diff --git a/system/blueberry/tests/gd_sl4a/lib/bt_constants.py b/system/blueberry/tests/gd_sl4a/lib/bt_constants.py
index 2dab54d..3ea669d 100644
--- a/system/blueberry/tests/gd_sl4a/lib/bt_constants.py
+++ b/system/blueberry/tests/gd_sl4a/lib/bt_constants.py
@@ -60,7 +60,6 @@
 bluetooth_off = "BluetoothStateChangedOff"
 bluetooth_on = "BluetoothStateChangedOn"
 mtu_changed = "GattConnect{}onMtuChanged"
-gatt_connection_state_change = "GattConnect{}onConnectionStateChange"
 advertising_set_started = "AdvertisingSet{}onAdvertisingSetStarted"
 advertising_set_stopped = "AdvertisingSet{}onAdvertisingSetStopped"
 advertising_set_on_own_address_read = "AdvertisingSet{}onOwnAddressRead"
@@ -261,274 +260,6 @@
 
 ### Bluetooth Low Energy Constants End ###
 
-### Bluetooth GATT Constants Begin ###
-
-# Gatt Callback error messages
-gatt_cb_err = {
-    "char_write_req_err": "Characteristic Write Request event not found. Expected {}",
-    "char_write_err": "Characteristic Write event not found. Expected {}",
-    "desc_write_req_err": "Descriptor Write Request event not found. Expected {}",
-    "desc_write_err": "Descriptor Write event not found. Expected {}",
-    "char_read_err": "Characteristic Read event not found. Expected {}",
-    "char_read_req_err": "Characteristic Read Request not found. Expected {}",
-    "desc_read_err": "Descriptor Read event not found. Expected {}",
-    "desc_read_req_err": "Descriptor Read Request event not found. Expected {}",
-    "rd_remote_rssi_err": "Read Remote RSSI event not found. Expected {}",
-    "gatt_serv_disc_err": "GATT Services Discovered event not found. Expected {}",
-    "serv_added_err": "Service Added event not found. Expected {}",
-    "mtu_changed_err": "MTU Changed event not found. Expected {}",
-    "mtu_serv_changed_err": "MTU Server Changed event not found. Expected {}",
-    "gatt_conn_changed_err": "GATT Connection Changed event not found. Expected {}",
-    "char_change_err": "GATT Characteristic Changed event not fond. Expected {}",
-    "phy_read_err": "Phy Read event not fond. Expected {}",
-    "phy_update_err": "Phy Update event not fond. Expected {}",
-    "exec_write_err": "GATT Execute Write event not found. Expected {}"
-}
-
-# GATT callback strings as defined in GattClientFacade.java and
-# GattServerFacade.java implemented callbacks.
-gatt_cb_strings = {
-    "char_write_req": "GattServer{}onCharacteristicWriteRequest",
-    "exec_write": "GattServer{}onExecuteWrite",
-    "char_write": "GattConnect{}onCharacteristicWrite",
-    "desc_write_req": "GattServer{}onDescriptorWriteRequest",
-    "desc_write": "GattConnect{}onDescriptorWrite",
-    "char_read": "GattConnect{}onCharacteristicRead",
-    "char_read_req": "GattServer{}onCharacteristicReadRequest",
-    "desc_read": "GattConnect{}onDescriptorRead",
-    "desc_read_req": "GattServer{}onDescriptorReadRequest",
-    "rd_remote_rssi": "GattConnect{}onReadRemoteRssi",
-    "gatt_serv_disc": "GattConnect{}onServicesDiscovered",
-    "serv_added": "GattServer{}onServiceAdded",
-    "mtu_changed": "GattConnect{}onMtuChanged",
-    "mtu_serv_changed": "GattServer{}onMtuChanged",
-    "gatt_conn_change": "GattConnect{}onConnectionStateChange",
-    "char_change": "GattConnect{}onCharacteristicChanged",
-    "phy_read": "GattConnect{}onPhyRead",
-    "phy_update": "GattConnect{}onPhyUpdate",
-    "serv_phy_read": "GattServer{}onPhyRead",
-    "serv_phy_update": "GattServer{}onPhyUpdate",
-}
-
-# GATT event dictionary of expected callbacks and errors.
-gatt_event = {
-    "char_write_req": {
-        "evt": gatt_cb_strings["char_write_req"],
-        "err": gatt_cb_err["char_write_req_err"]
-    },
-    "exec_write": {
-        "evt": gatt_cb_strings["exec_write"],
-        "err": gatt_cb_err["exec_write_err"]
-    },
-    "char_write": {
-        "evt": gatt_cb_strings["char_write"],
-        "err": gatt_cb_err["char_write_err"]
-    },
-    "desc_write_req": {
-        "evt": gatt_cb_strings["desc_write_req"],
-        "err": gatt_cb_err["desc_write_req_err"]
-    },
-    "desc_write": {
-        "evt": gatt_cb_strings["desc_write"],
-        "err": gatt_cb_err["desc_write_err"]
-    },
-    "char_read": {
-        "evt": gatt_cb_strings["char_read"],
-        "err": gatt_cb_err["char_read_err"]
-    },
-    "char_read_req": {
-        "evt": gatt_cb_strings["char_read_req"],
-        "err": gatt_cb_err["char_read_req_err"]
-    },
-    "desc_read": {
-        "evt": gatt_cb_strings["desc_read"],
-        "err": gatt_cb_err["desc_read_err"]
-    },
-    "desc_read_req": {
-        "evt": gatt_cb_strings["desc_read_req"],
-        "err": gatt_cb_err["desc_read_req_err"]
-    },
-    "rd_remote_rssi": {
-        "evt": gatt_cb_strings["rd_remote_rssi"],
-        "err": gatt_cb_err["rd_remote_rssi_err"]
-    },
-    "gatt_serv_disc": {
-        "evt": gatt_cb_strings["gatt_serv_disc"],
-        "err": gatt_cb_err["gatt_serv_disc_err"]
-    },
-    "serv_added": {
-        "evt": gatt_cb_strings["serv_added"],
-        "err": gatt_cb_err["serv_added_err"]
-    },
-    "mtu_changed": {
-        "evt": gatt_cb_strings["mtu_changed"],
-        "err": gatt_cb_err["mtu_changed_err"]
-    },
-    "gatt_conn_change": {
-        "evt": gatt_cb_strings["gatt_conn_change"],
-        "err": gatt_cb_err["gatt_conn_changed_err"]
-    },
-    "char_change": {
-        "evt": gatt_cb_strings["char_change"],
-        "err": gatt_cb_err["char_change_err"]
-    },
-    "phy_read": {
-        "evt": gatt_cb_strings["phy_read"],
-        "err": gatt_cb_err["phy_read_err"]
-    },
-    "phy_update": {
-        "evt": gatt_cb_strings["phy_update"],
-        "err": gatt_cb_err["phy_update_err"]
-    },
-    "serv_phy_read": {
-        "evt": gatt_cb_strings["serv_phy_read"],
-        "err": gatt_cb_err["phy_read_err"]
-    },
-    "serv_phy_update": {
-        "evt": gatt_cb_strings["serv_phy_update"],
-        "err": gatt_cb_err["phy_update_err"]
-    }
-}
-
-# Matches constants of connection states defined in BluetoothGatt.java
-gatt_connection_state = {"disconnected": 0, "connecting": 1, "connected": 2, "disconnecting": 3, "closed": 4}
-
-# Matches constants of Bluetooth GATT Characteristic values as defined
-# in BluetoothGattCharacteristic.java
-gatt_characteristic = {
-    "property_broadcast": 0x01,
-    "property_read": 0x02,
-    "property_write_no_response": 0x04,
-    "property_write": 0x08,
-    "property_notify": 0x10,
-    "property_indicate": 0x20,
-    "property_signed_write": 0x40,
-    "property_extended_props": 0x80,
-    "permission_read": 0x01,
-    "permission_read_encrypted": 0x02,
-    "permission_read_encrypted_mitm": 0x04,
-    "permission_write": 0x10,
-    "permission_write_encrypted": 0x20,
-    "permission_write_encrypted_mitm": 0x40,
-    "permission_write_signed": 0x80,
-    "permission_write_signed_mitm": 0x100,
-    "write_type_default": 0x02,
-    "write_type_no_response": 0x01,
-    "write_type_signed": 0x04,
-}
-
-# Matches constants of Bluetooth GATT Characteristic values as defined
-# in BluetoothGattDescriptor.java
-gatt_descriptor = {
-    "enable_notification_value": [0x01, 0x00],
-    "enable_indication_value": [0x02, 0x00],
-    "disable_notification_value": [0x00, 0x00],
-    "permission_read": 0x01,
-    "permission_read_encrypted": 0x02,
-    "permission_read_encrypted_mitm": 0x04,
-    "permission_write": 0x10,
-    "permission_write_encrypted": 0x20,
-    "permission_write_encrypted_mitm": 0x40,
-    "permission_write_signed": 0x80,
-    "permission_write_signed_mitm": 0x100
-}
-
-# https://www.bluetooth.com/specifications/gatt/descriptors
-gatt_char_desc_uuids = {
-    "char_ext_props": '00002900-0000-1000-8000-00805f9b34fb',
-    "char_user_desc": '00002901-0000-1000-8000-00805f9b34fb',
-    "client_char_cfg": '00002902-0000-1000-8000-00805f9b34fb',
-    "server_char_cfg": '00002903-0000-1000-8000-00805f9b34fb',
-    "char_fmt_uuid": '00002904-0000-1000-8000-00805f9b34fb',
-    "char_agreg_fmt": '00002905-0000-1000-8000-00805f9b34fb',
-    "char_valid_range": '00002906-0000-1000-8000-00805f9b34fb',
-    "external_report_reference": '00002907-0000-1000-8000-00805f9b34fb',
-    "report_reference": '00002908-0000-1000-8000-00805f9b34fb'
-}
-
-# https://www.bluetooth.com/specifications/gatt/characteristics
-gatt_char_types = {
-    "device_name": '00002a00-0000-1000-8000-00805f9b34fb',
-    "appearance": '00002a01-0000-1000-8000-00805f9b34fb',
-    "peripheral_priv_flag": '00002a02-0000-1000-8000-00805f9b34fb',
-    "reconnection_address": '00002a03-0000-1000-8000-00805f9b34fb',
-    "peripheral_pref_conn": '00002a04-0000-1000-8000-00805f9b34fb',
-    "service_changed": '00002a05-0000-1000-8000-00805f9b34fb',
-    "system_id": '00002a23-0000-1000-8000-00805f9b34fb',
-    "model_number_string": '00002a24-0000-1000-8000-00805f9b34fb',
-    "serial_number_string": '00002a25-0000-1000-8000-00805f9b34fb',
-    "firmware_revision_string": '00002a26-0000-1000-8000-00805f9b34fb',
-    "hardware_revision_string": '00002a27-0000-1000-8000-00805f9b34fb',
-    "software_revision_string": '00002a28-0000-1000-8000-00805f9b34fb',
-    "manufacturer_name_string": '00002a29-0000-1000-8000-00805f9b34fb',
-    "pnp_id": '00002a50-0000-1000-8000-00805f9b34fb',
-}
-
-# Matches constants of Bluetooth GATT Characteristic values as defined
-# in BluetoothGattCharacteristic.java
-gatt_characteristic_value_format = {
-    "string": 0x1,
-    "byte": 0x2,
-    "sint8": 0x21,
-    "uint8": 0x11,
-    "sint16": 0x22,
-    "unit16": 0x12,
-    "sint32": 0x24,
-    "uint32": 0x14
-}
-
-# Matches constants of Bluetooth Gatt Service types as defined in
-# BluetoothGattService.java
-gatt_service_types = {"primary": 0, "secondary": 1}
-
-# Matches constants of Bluetooth Gatt Connection Priority values as defined in
-# BluetoothGatt.java
-gatt_connection_priority = {"balanced": 0, "high": 1, "low_power": 2}
-
-# Min and max MTU values
-gatt_mtu_size = {"min": 23, "max": 217}
-
-# Gatt Characteristic attribute lengths
-gatt_characteristic_attr_length = {"attr_1": 1, "attr_2": 3, "attr_3": 15}
-
-# Matches constants of Bluetooth Gatt operations status as defined in
-# BluetoothGatt.java
-gatt_status = {"success": 0, "failure": 0x101}
-
-# Matches constants of Bluetooth transport values as defined in
-# BluetoothDevice.java
-gatt_transport = {"auto": 0x00, "bredr": 0x01, "le": 0x02}
-
-# Matches constants of Bluetooth physical channeling values as defined in
-# BluetoothDevice.java
-gatt_phy = {"1m": 1, "2m": 2, "le_coded": 3}
-
-# Matches constants of Bluetooth physical channeling bitmask values as defined
-# in BluetoothDevice.java
-gatt_phy_mask = {"1m_mask": 1, "2m_mask": 2, "coded_mask": 4}
-
-# Values as defiend in the Bluetooth GATT specification
-gatt_server_responses = {
-    "GATT_SUCCESS": 0x0,
-    "GATT_FAILURE": 0x1,
-    "GATT_READ_NOT_PERMITTED": 0x2,
-    "GATT_WRITE_NOT_PERMITTED": 0x3,
-    "GATT_INVALID_PDU": 0x4,
-    "GATT_INSUFFICIENT_AUTHENTICATION": 0x5,
-    "GATT_REQUEST_NOT_SUPPORTED": 0x6,
-    "GATT_INVALID_OFFSET": 0x7,
-    "GATT_INSUFFICIENT_AUTHORIZATION": 0x8,
-    "GATT_INVALID_ATTRIBUTE_LENGTH": 0xd,
-    "GATT_INSUFFICIENT_ENCRYPTION": 0xf,
-    "GATT_CONNECTION_CONGESTED": 0x8f,
-    "GATT_13_ERR": 0x13,
-    "GATT_12_ERR": 0x12,
-    "GATT_0C_ERR": 0x0C,
-    "GATT_16": 0x16
-}
-
-### Bluetooth GATT Constants End ###
-
 ### Chameleon Constants Begin ###
 
 # Chameleon audio bits per sample.
diff --git a/system/blueberry/tests/gd_sl4a/lib/gd_sl4a_base_test.py b/system/blueberry/tests/gd_sl4a/lib/gd_sl4a_base_test.py
index a5ae587..899df00 100644
--- a/system/blueberry/tests/gd_sl4a/lib/gd_sl4a_base_test.py
+++ b/system/blueberry/tests/gd_sl4a/lib/gd_sl4a_base_test.py
@@ -26,11 +26,9 @@
 from mobly.controllers.android_device_lib.adb import AdbError
 from mobly.controllers import android_device
 from mobly.controllers.android_device import MOBLY_CONTROLLER_CONFIG_NAME as ANDROID_DEVICE_COFNIG_NAME
-from mobly.controllers.android_device_lib.jsonrpc_client_base import \
-    AppRestoreConnectionError
-from mobly.controllers.android_device_lib.services.sl4a_service import Sl4aService
-import mobly.controllers.android_device_lib.sl4a_client as sl4a_client
 
+from blueberry.utils.mobly_sl4a_utils import setup_sl4a
+from blueberry.utils.mobly_sl4a_utils import teardown_sl4a
 from blueberry.tests.gd.cert.context import get_current_context
 from blueberry.tests.gd.cert.gd_device import MOBLY_CONTROLLER_CONFIG_NAME as GD_DEVICE_CONFIG_NAME
 from blueberry.tests.gd_sl4a.lib.ble_lib import enable_bluetooth, disable_bluetooth, BleLib
@@ -54,25 +52,14 @@
         self.cert = self.gd_devices[0]
 
         # Parse and construct Android device objects
+        self.android_devices = self.register_controller(android_device, required=True)
         server_port = int(self.controller_configs[ANDROID_DEVICE_COFNIG_NAME][0]['server_port'])
         forwarded_port = int(self.controller_configs[ANDROID_DEVICE_COFNIG_NAME][0]['forwarded_port'])
-        sl4a_client._DEVICE_SIDE_PORT = server_port
-        sl4a_client._APP_START_WAIT_TIME = 0.5
-        self.android_devices = self.register_controller(android_device, required=True)
         self.dut = self.android_devices[0]
-        self.dut.services.register('sl4a', Sl4aService, start_service=False)
-        try:
-            self.dut.sl4a.start()
-        except AppRestoreConnectionError:
-            pass
-        try:
-            self.dut.sl4a.clear_host_port()
-        except AdbError:
-            pass
-        sl4a_client._APP_START_WAIT_TIME = 2 * 60
-        self.dut.sl4a.restore_app_connection(port=forwarded_port)
+        setup_sl4a(self.dut, server_port, forwarded_port)
 
         # Enable full btsnoop log
+        self.dut.adb.root()
         self.dut.adb.shell("setprop persist.bluetooth.btsnooplogmode full")
         getprop_result = self.dut.adb.shell("getprop persist.bluetooth.btsnooplogmode") == "full"
         if not getprop_result:
@@ -81,7 +68,7 @@
         self.ble = BleLib(dut=self.dut)
 
     def teardown_class(self):
-        pass
+        teardown_sl4a(self.dut)
 
     def setup_test(self):
         self.cert.rootservice.StartStack(
diff --git a/system/blueberry/tests/sl4a_sl4a/gatt/gatt_connect_test.py b/system/blueberry/tests/sl4a_sl4a/gatt/gatt_connect_test.py
index 1d21bc6..a2dfea1 100644
--- a/system/blueberry/tests/sl4a_sl4a/gatt/gatt_connect_test.py
+++ b/system/blueberry/tests/sl4a_sl4a/gatt/gatt_connect_test.py
@@ -1,141 +1,1006 @@
 #!/usr/bin/env python3
 #
-#   Copyright 2021 - The Android Open Source Project
+# Copyright (C) 2016 The Android Open Source Project
 #
-#   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
+# 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
+# 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.
+# 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.
+"""
+This test script exercises different GATT connection tests.
 
-import io
-import os
-import queue
+Original location:
+  tools/test/connectivity/acts_tests/tests/google/ble/gatt/GattConnectTest.py
+"""
+
 import logging
+import time
+from queue import Empty
 
-from blueberry.tests.gd.cert.context import get_current_context
-from blueberry.tests.gd.cert.truth import assertThat
-from blueberry.tests.gd_sl4a.lib.bt_constants import adv_succ, ble_advertise_settings_modes, ble_scan_settings_modes, ble_address_types, ble_scan_settings_phys, gatt_connection_state_change, gatt_transport, scan_result
-from blueberry.tests.gd_sl4a.lib.ble_lib import generate_ble_scan_objects, generate_ble_advertise_objects
+from blueberry.tests.gd.cert.test_decorators import test_tracker_info
+from blueberry.tests.gd_sl4a.lib.bt_constants import scan_result
 from blueberry.tests.sl4a_sl4a.lib import sl4a_sl4a_base_test
-from blueberry.facade import common_pb2 as common
-
+from blueberry.utils.ble_scan_adv_constants import BleAdvertiseSettingsMode
+from blueberry.utils.ble_scan_adv_constants import BleScanSettingsMatchNums
+from blueberry.utils.ble_scan_adv_constants import BleScanSettingsModes
+from blueberry.utils.bt_constants import BluetoothProfile
+from blueberry.utils.bt_gatt_constants import GattCallbackError
+from blueberry.utils.bt_gatt_constants import GattCallbackString
+from blueberry.utils.bt_gatt_constants import GattCharacteristic
+from blueberry.utils.bt_gatt_constants import GattConnectionState
+from blueberry.utils.bt_gatt_constants import GattMtuSize
+from blueberry.utils.bt_gatt_constants import GattPhyMask
+from blueberry.utils.bt_gatt_constants import GattServiceType
+from blueberry.utils.bt_gatt_constants import GattTransport
+from blueberry.utils.bt_gatt_utils import GattTestUtilsError
+from blueberry.utils.bt_gatt_utils import close_gatt_client
+from blueberry.utils.bt_gatt_utils import disconnect_gatt_connection
+from blueberry.utils.bt_gatt_utils import get_mac_address_of_generic_advertisement
+from blueberry.utils.bt_gatt_utils import log_gatt_server_uuids
+from blueberry.utils.bt_gatt_utils import orchestrate_gatt_connection
+from blueberry.utils.bt_gatt_utils import setup_gatt_connection
+from blueberry.utils.bt_gatt_utils import setup_multiple_services
+from blueberry.utils.bt_gatt_utils import wait_for_gatt_disconnect_event
+from blueberry.utils.bt_test_utils import clear_bonded_devices
 from mobly import test_runner
-from mobly.controllers.android_device_lib.adb import AdbError
+
+PHYSICAL_DISCONNECT_TIMEOUT = 5
 
 
 class GattConnectTest(sl4a_sl4a_base_test.Sl4aSl4aBaseTestClass):
+    adv_instances = []
+    bluetooth_gatt_list = []
+    gatt_server_list = []
+    default_timeout = 10
+    default_discovery_timeout = 3
 
     def setup_class(self):
         super().setup_class()
-        self.default_timeout = 10  # seconds
+        self.central = self.dut
+        self.peripheral = self.cert
 
     def setup_test(self):
         super().setup_test()
+        bluetooth_gatt_list = []
+        self.gatt_server_list = []
+        self.adv_instances = []
+        # Ensure there is ample time for a physical disconnect in between
+        # testcases.
+        logging.info("Waiting for {} seconds for physical GATT disconnections".format(PHYSICAL_DISCONNECT_TIMEOUT))
+        time.sleep(PHYSICAL_DISCONNECT_TIMEOUT)
 
     def teardown_test(self):
+        for bluetooth_gatt in self.bluetooth_gatt_list:
+            self.central.sl4a.gattClientClose(bluetooth_gatt)
+        for gatt_server in self.gatt_server_list:
+            self.peripheral.sl4a.gattServerClose(gatt_server)
+        for adv in self.adv_instances:
+            self.peripheral.sl4a.bleStopBleAdvertising(adv)
         super().teardown_test()
+        return True
 
-    def _wait_for_event(self, expected_event_name, device):
+    def _orchestrate_gatt_disconnection(self, bluetooth_gatt, gatt_callback):
+        logging.info("Disconnecting from peripheral device.")
         try:
-            event_info = device.ed.pop_event(expected_event_name, self.default_timeout)
-            logging.info(event_info)
-        except queue.Empty as error:
-            logging.error("Failed to find event: %s", expected_event_name)
+            disconnect_gatt_connection(self.central, bluetooth_gatt, gatt_callback)
+            close_gatt_client(self.central, bluetooth_gatt)
+            if bluetooth_gatt in self.bluetooth_gatt_list:
+                self.bluetooth_gatt_list.remove(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
             return False
         return True
 
-    def _wait_for_scan_result_event(self, expected_event_name, device):
+    def _find_service_added_event(self, gatt_server_cb, uuid):
+        expected_event = GattCallbackString.SERV_ADDED.format(gatt_server_cb)
         try:
-            event_info = device.ed.pop_event(expected_event_name, self.default_timeout)
-        except queue.Empty as error:
-            logging.error("Could not find scan result event: %s", expected_event_name)
-            return None
-        return event_info['data']['Result']['deviceInfo']['address']
-
-    def _get_cert_public_address_and_irk_from_bt_config(self):
-        # Pull IRK from SL4A cert side to pass in from SL4A DUT side when scanning
-        bt_config_file_path = os.path.join(get_current_context().get_full_output_path(),
-                                           "DUT_%s_bt_config.conf" % self.cert.serial)
-        try:
-            self.cert.adb.pull(["/data/misc/bluedroid/bt_config.conf", bt_config_file_path])
-        except AdbError as error:
-            logging.error("Failed to pull SL4A cert BT config")
+            event = self.peripheral.ed.pop_event(expected_event, self.default_timeout)
+        except Empty:
+            logging.error(GattCallbackError.SERV_ADDED_ERR.format(expected_event))
             return False
-        logging.debug("Reading SL4A cert BT config")
-        with io.open(bt_config_file_path) as f:
-            for line in f.readlines():
-                stripped_line = line.strip()
-                if (stripped_line.startswith("Address")):
-                    address_fields = stripped_line.split(' ')
-                    # API currently requires public address to be capitalized
-                    address = address_fields[2].upper()
-                    logging.debug("Found cert address: %s" % address)
-                    continue
-                if (stripped_line.startswith("LE_LOCAL_KEY_IRK")):
-                    irk_fields = stripped_line.split(' ')
-                    irk = irk_fields[2]
-                    logging.debug("Found cert IRK: %s" % irk)
-                    continue
+        if event['data']['serviceUuid'].lower() != uuid.lower():
+            logging.error("Uuid mismatch. Found: {}, Expected {}.".format(event['data']['serviceUuid'], uuid))
+            return False
+        return True
 
-        return address, irk
+    def _verify_mtu_changed_on_client_and_server(self, expected_mtu, gatt_callback, gatt_server_callback):
+        expected_event = GattCallbackString.MTU_CHANGED.format(gatt_callback)
+        try:
+            mtu_event = self.central.ed.pop_event(expected_event, self.default_timeout)
+            mtu_size_found = mtu_event['data']['MTU']
+            if mtu_size_found != expected_mtu:
+                logging.error("MTU size found: {}, expected: {}".format(mtu_size_found, expected_mtu))
+                return False
+        except Empty:
+            logging.error(GattCallbackError.MTU_CHANGED_ERR.format(expected_event))
+            return False
 
-    def test_scan_connect_unbonded_device_public_address_with_irk(self):
-        # Set up SL4A cert side to advertise
-        logging.info("Starting advertising")
-        self.cert.sl4a.bleSetAdvertiseSettingsIsConnectable(True)
-        self.cert.sl4a.bleSetAdvertiseDataIncludeDeviceName(True)
-        self.cert.sl4a.bleSetAdvertiseSettingsAdvertiseMode(ble_advertise_settings_modes['low_latency'])
-        self.cert.sl4a.bleSetAdvertiseSettingsOwnAddressType(common.RANDOM_DEVICE_ADDRESS)
-        advertise_callback, advertise_data, advertise_settings = generate_ble_advertise_objects(self.cert.sl4a)
-        self.cert.sl4a.bleStartBleAdvertising(advertise_callback, advertise_data, advertise_settings)
+        expected_event = GattCallbackString.MTU_SERV_CHANGED.format(gatt_server_callback)
+        try:
+            mtu_event = self.peripheral.ed.pop_event(expected_event, self.default_timeout)
+            mtu_size_found = mtu_event['data']['MTU']
+            if mtu_size_found != expected_mtu:
+                logging.error("MTU size found: {}, expected: {}".format(mtu_size_found, expected_mtu))
+                return False
+        except Empty:
+            logging.error(GattCallbackError.MTU_SERV_CHANGED_ERR.format(expected_event))
+            return False
+        return True
 
-        # Wait for SL4A cert to start advertising
-        assertThat(self._wait_for_event(adv_succ.format(advertise_callback), self.cert)).isTrue()
-        logging.info("Advertising started")
+    @test_tracker_info(uuid='8a3530a3-c8bb-466b-9710-99e694c38618')
+    def test_gatt_connect(self):
+        """Test GATT connection over LE.
 
-        # Pull IRK from SL4A cert side to pass in from SL4A DUT side when scanning
-        cert_public_address, irk = self._get_cert_public_address_and_irk_from_bt_config()
+          Test establishing a gatt connection between a GATT server and GATT
+          client.
 
-        # Set up SL4A DUT side to scan
-        addr_type = ble_address_types["public"]
-        logging.info("Start scanning for PUBLIC_ADDRESS %s with address type %d and IRK %s" % (cert_public_address,
-                                                                                               addr_type, irk))
-        self.dut.sl4a.bleSetScanSettingsScanMode(ble_scan_settings_modes['low_latency'])
-        self.dut.sl4a.bleSetScanSettingsLegacy(False)
-        filter_list, scan_settings, scan_callback = generate_ble_scan_objects(self.dut.sl4a)
+          Steps:
+            1. Start a generic advertisement.
+            2. Start a generic scanner.
+            3. Find the advertisement and extract the mac address.
+            4. Stop the first scanner.
+            5. Create a GATT connection between the scanner and advertiser.
+            6. Disconnect the GATT connection.
+
+          Expected Result:
+            Verify that a connection was established and then disconnected
+            successfully.
+
+          Returns:
+            Pass if True
+            Fail if False
+
+          TAGS: LE, Advertising, Filtering, Scanning, GATT
+          Priority: 0
+          """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.adv_instances.append(adv_callback)
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='a839b505-03ac-4783-be7e-1d43129a1948')
+    def test_gatt_connect_stop_advertising(self):
+        """Test GATT connection over LE then stop advertising
+
+          A test case that verifies the GATT connection doesn't
+          disconnect when LE advertisement is stopped.
+
+          Steps:
+            1. Start a generic advertisement.
+            2. Start a generic scanner.
+            3. Find the advertisement and extract the mac address.
+            4. Stop the first scanner.
+            5. Create a GATT connection between the scanner and advertiser.
+            6. Stop the advertiser.
+            7. Verify no connection state changed happened.
+            8. Disconnect the GATT connection.
+
+          Expected Result:
+            Verify that a connection was established and not disconnected
+            when advertisement stops.
+
+          Returns:
+            Pass if True
+            Fail if False
+
+          TAGS: LE, Advertising, Filtering, Scanning, GATT
+          Priority: 0
+          """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.peripheral.sl4a.bleStopBleAdvertising(adv_callback)
+        try:
+            event = self.central.ed.pop_event(
+                GattCallbackString.GATT_CONN_CHANGE.format(gatt_callback, self.default_timeout))
+            logging.error("Connection event found when not expected: {}".format(event))
+            return False
+        except Empty:
+            logging.info("No connection state change as expected")
+        try:
+            self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+        except Exception as err:
+            logging.info("Failed to orchestrate disconnect: {}".format(err))
+            return False
+        return True
+
+    @test_tracker_info(uuid='b82f91a8-54bb-4779-a117-73dc7fdb28cc')
+    def test_gatt_connect_autoconnect(self):
+        """Test GATT connection over LE.
+
+          Test re-establishing a gatt connection using autoconnect
+          set to True in order to test connection allowlist.
+
+          Steps:
+            1. Start a generic advertisement.
+            2. Start a generic scanner.
+            3. Find the advertisement and extract the mac address.
+            4. Stop the first scanner.
+            5. Create a GATT connection between the scanner and advertiser.
+            6. Disconnect the GATT connection.
+            7. Create a GATT connection with autoconnect set to True
+            8. Disconnect the GATT connection.
+
+          Expected Result:
+            Verify that a connection was re-established and then disconnected
+            successfully.
+
+          Returns:
+            Pass if True
+            Fail if False
+
+          TAGS: LE, Advertising, Filtering, Scanning, GATT
+          Priority: 0
+          """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        autoconnect = False
+        mac_address, adv_callback, scan_callback = (get_mac_address_of_generic_advertisement(
+            self.central, self.peripheral))
+        self.adv_instances.append(adv_callback)
+        try:
+            bluetooth_gatt, gatt_callback = setup_gatt_connection(self.central, mac_address, autoconnect)
+            self.central.sl4a.bleStopBleScan(scan_callback)
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        try:
+            disconnect_gatt_connection(self.central, bluetooth_gatt, gatt_callback)
+            close_gatt_client(self.central, bluetooth_gatt)
+            if bluetooth_gatt in self.bluetooth_gatt_list:
+                self.bluetooth_gatt_list.remove(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        autoconnect = True
+        bluetooth_gatt = self.central.sl4a.gattClientConnectGatt(
+            gatt_callback, mac_address, autoconnect, GattTransport.TRANSPORT_AUTO, False, GattPhyMask.PHY_LE_1M_MASK)
+        self.bluetooth_gatt_list.append(bluetooth_gatt)
+        expected_event = GattCallbackString.GATT_CONN_CHANGE.format(gatt_callback)
+        try:
+            event = self.central.ed.pop_event(expected_event, self.default_timeout)
+        except Empty:
+            logging.error(GattCallbackError.GATT_CONN_CHANGE_ERR.format(expected_event))
+            test_result = False
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='e506fa50-7cd9-4bd8-938a-6b85dcfe6bc6')
+    def test_gatt_connect_opportunistic(self):
+        """Test opportunistic GATT connection over LE.
+
+          Test establishing a gatt connection between a GATT server and GATT
+          client in opportunistic mode.
+
+          Steps:
+            1. Start a generic advertisement.
+            2. Start a generic scanner.
+            3. Find the advertisement and extract the mac address.
+            4. Stop the first scanner.
+            5. Create GATT connection 1 between the scanner and advertiser normally
+            6. Create GATT connection 2 between the scanner and advertiser using
+               opportunistic mode
+            7. Disconnect GATT connection 1
+
+          Expected Result:
+            Verify GATT connection 2 automatically disconnects when GATT connection
+            1 disconnect
+
+          Returns:
+            Pass if True
+            Fail if False
+
+          TAGS: LE, Advertising, Filtering, Scanning, GATT
+          Priority: 0
+          """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        mac_address, adv_callback, scan_callback = (get_mac_address_of_generic_advertisement(
+            self.central, self.peripheral))
+        # Make GATT connection 1
+        try:
+            bluetooth_gatt_1, gatt_callback_1 = setup_gatt_connection(
+                self.central, mac_address, False, transport=GattTransport.TRANSPORT_AUTO, opportunistic=False)
+            self.central.sl4a.bleStopBleScan(scan_callback)
+            self.adv_instances.append(adv_callback)
+            self.bluetooth_gatt_list.append(bluetooth_gatt_1)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        # Make GATT connection 2
+        try:
+            bluetooth_gatt_2, gatt_callback_2 = setup_gatt_connection(
+                self.central, mac_address, False, transport=GattTransport.TRANSPORT_AUTO, opportunistic=True)
+            self.bluetooth_gatt_list.append(bluetooth_gatt_2)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        # Disconnect GATT connection 1
+        try:
+            disconnect_gatt_connection(self.central, bluetooth_gatt_1, gatt_callback_1)
+            close_gatt_client(self.central, bluetooth_gatt_1)
+            if bluetooth_gatt_1 in self.bluetooth_gatt_list:
+                self.bluetooth_gatt_list.remove(bluetooth_gatt_1)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        # Confirm that GATT connection 2 also disconnects
+        wait_for_gatt_disconnect_event(self.central, gatt_callback_2)
+        close_gatt_client(self.central, bluetooth_gatt_2)
+        if bluetooth_gatt_2 in self.bluetooth_gatt_list:
+            self.bluetooth_gatt_list.remove(bluetooth_gatt_2)
+        return True
+
+    @test_tracker_info(uuid='1e01838e-c4de-4720-9adf-9e0419378226')
+    def test_gatt_request_min_mtu(self):
+        """Test GATT connection over LE and exercise MTU sizes.
+
+          Test establishing a gatt connection between a GATT server and GATT
+          client. Request an MTU size that matches the correct minimum size.
+
+          Steps:
+            1. Start a generic advertisement.
+            2. Start a generic scanner.
+            3. Find the advertisement and extract the mac address.
+            4. Stop the first scanner.
+            5. Create a GATT connection between the scanner and advertiser.
+            6. From the scanner (client) request MTU size change to the
+            minimum value.
+            7. Find the MTU changed event on the client.
+            8. Disconnect the GATT connection.
+
+          Expected Result:
+            Verify that a connection was established and the MTU value found
+            matches the expected MTU value.
+
+          Returns:
+            Pass if True
+            Fail if False
+
+          TAGS: LE, Advertising, Filtering, Scanning, GATT, MTU
+          Priority: 0
+          """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.adv_instances.append(adv_callback)
+        expected_mtu = GattMtuSize.MIN
+        self.central.sl4a.gattClientRequestMtu(bluetooth_gatt, expected_mtu)
+        if not self._verify_mtu_changed_on_client_and_server(expected_mtu, gatt_callback, gatt_server_cb):
+            return False
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='c1fa3a2d-fb47-47db-bdd1-458928cd6a5f')
+    def test_gatt_request_max_mtu(self):
+        """Test GATT connection over LE and exercise MTU sizes.
+
+          Test establishing a gatt connection between a GATT server and GATT
+          client. Request an MTU size that matches the correct maximum size.
+
+          Steps:
+            1. Start a generic advertisement.
+            2. Start a generic scanner.
+            3. Find the advertisement and extract the mac address.
+            4. Stop the first scanner.
+            5. Create a GATT connection between the scanner and advertiser.
+            6. From the scanner (client) request MTU size change to the
+            maximum value.
+            7. Find the MTU changed event on the client.
+            8. Disconnect the GATT connection.
+
+          Expected Result:
+            Verify that a connection was established and the MTU value found
+            matches the expected MTU value.
+
+          Returns:
+            Pass if True
+            Fail if False
+
+          TAGS: LE, Advertising, Filtering, Scanning, GATT, MTU
+          Priority: 0
+          """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.adv_instances.append(adv_callback)
+        expected_mtu = GattMtuSize.MAX
+        self.central.sl4a.gattClientRequestMtu(bluetooth_gatt, expected_mtu)
+        if not self._verify_mtu_changed_on_client_and_server(expected_mtu, gatt_callback, gatt_server_cb):
+            return False
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='4416d483-dec3-46cb-8038-4d82620f873a')
+    def test_gatt_request_out_of_bounds_mtu(self):
+        """Test GATT connection over LE and exercise an out of bound MTU size.
+
+          Test establishing a gatt connection between a GATT server and GATT
+          client. Request an MTU size that is the MIN value minus 1.
+
+          Steps:
+            1. Start a generic advertisement.
+            2. Start a generic scanner.
+            3. Find the advertisement and extract the mac address.
+            4. Stop the first scanner.
+            5. Create a GATT connection between the scanner and advertiser.
+            6. From the scanner (client) request MTU size change to the
+            minimum value minus one.
+            7. Find the MTU changed event on the client.
+            8. Disconnect the GATT connection.
+
+          Expected Result:
+            Verify that an MTU changed event was not discovered and that
+            it didn't cause an exception when requesting an out of bounds
+            MTU.
+
+          Returns:
+            Pass if True
+            Fail if False
+
+          TAGS: LE, Advertising, Filtering, Scanning, GATT, MTU
+          Priority: 0
+          """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.adv_instances.append(adv_callback)
+        unexpected_mtu = GattMtuSize.MIN - 1
+        self.central.sl4a.gattClientRequestMtu(bluetooth_gatt, unexpected_mtu)
+        if self._verify_mtu_changed_on_client_and_server(unexpected_mtu, gatt_callback, gatt_server_cb):
+            return False
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='31ffb9ca-cc75-43fb-9802-c19f1c5856b6')
+    def test_gatt_connect_trigger_on_read_rssi(self):
+        """Test GATT connection over LE read RSSI.
+
+        Test establishing a gatt connection between a GATT server and GATT
+        client then read the RSSI.
+
+        Steps:
+          1. Start a generic advertisement.
+          2. Start a generic scanner.
+          3. Find the advertisement and extract the mac address.
+          4. Stop the first scanner.
+          5. Create a GATT connection between the scanner and advertiser.
+          6. From the scanner, request to read the RSSI of the advertiser.
+          7. Disconnect the GATT connection.
+
+        Expected Result:
+          Verify that a connection was established and then disconnected
+          successfully. Verify that the RSSI was ready correctly.
+
+        Returns:
+          Pass if True
+          Fail if False
+
+        TAGS: LE, Advertising, Filtering, Scanning, GATT, RSSI
+        Priority: 1
+        """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.adv_instances.append(adv_callback)
+        expected_event = GattCallbackString.RD_REMOTE_RSSI.format(gatt_callback)
+        if self.central.sl4a.gattClientReadRSSI(bluetooth_gatt):
+            try:
+                self.central.ed.pop_event(expected_event, self.default_timeout)
+            except Empty:
+                logging.error(GattCallbackError.RD_REMOTE_RSSI_ERR.format(expected_event))
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='dee9ef28-b872-428a-821b-cc62f27ba936')
+    def test_gatt_connect_trigger_on_services_discovered(self):
+        """Test GATT connection and discover services of peripheral.
+
+          Test establishing a gatt connection between a GATT server and GATT
+          client the discover all services from the connected device.
+
+          Steps:
+            1. Start a generic advertisement.
+            2. Start a generic scanner.
+            3. Find the advertisement and extract the mac address.
+            4. Stop the first scanner.
+            5. Create a GATT connection between the scanner and advertiser.
+            6. From the scanner (central device), discover services.
+            7. Disconnect the GATT connection.
+
+          Expected Result:
+            Verify that a connection was established and then disconnected
+            successfully. Verify that the service were discovered.
+
+          Returns:
+            Pass if True
+            Fail if False
+
+          TAGS: LE, Advertising, Filtering, Scanning, GATT, Services
+          Priority: 1
+          """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.adv_instances.append(adv_callback)
+        if self.central.sl4a.gattClientDiscoverServices(bluetooth_gatt):
+            expected_event = GattCallbackString.GATT_SERV_DISC.format(gatt_callback)
+            try:
+                event = self.central.ed.pop_event(expected_event, self.default_timeout)
+            except Empty:
+                logging.error(GattCallbackError.GATT_SERV_DISC_ERR.format(expected_event))
+                return False
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='01883bdd-0cf8-48fb-bf15-467bbd4f065b')
+    def test_gatt_connect_trigger_on_services_discovered_iterate_attributes(self):
+        """Test GATT connection and iterate peripherals attributes.
+
+        Test establishing a gatt connection between a GATT server and GATT
+        client and iterate over all the characteristics and descriptors of the
+        discovered services.
+
+        Steps:
+          1. Start a generic advertisement.
+          2. Start a generic scanner.
+          3. Find the advertisement and extract the mac address.
+          4. Stop the first scanner.
+          5. Create a GATT connection between the scanner and advertiser.
+          6. From the scanner (central device), discover services.
+          7. Iterate over all the characteristics and descriptors of the
+          discovered features.
+          8. Disconnect the GATT connection.
+
+        Expected Result:
+          Verify that a connection was established and then disconnected
+          successfully. Verify that the services, characteristics, and descriptors
+          were discovered.
+
+        Returns:
+          Pass if True
+          Fail if False
+
+        TAGS: LE, Advertising, Filtering, Scanning, GATT, Services
+        Characteristics, Descriptors
+        Priority: 1
+        """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.adv_instances.append(adv_callback)
+        if self.central.sl4a.gattClientDiscoverServices(bluetooth_gatt):
+            expected_event = GattCallbackString.GATT_SERV_DISC.format(gatt_callback)
+            try:
+                event = self.central.ed.pop_event(expected_event, self.default_timeout)
+                discovered_services_index = event['data']['ServicesIndex']
+            except Empty:
+                logging.error(GattCallbackError.GATT_SERV_DISC_ERR.format(expected_event))
+                return False
+            log_gatt_server_uuids(self.central, discovered_services_index)
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='d4277bee-da99-4f48-8a4d-f81b5389da18')
+    def test_gatt_connect_with_service_uuid_variations(self):
+        """Test GATT connection with multiple service uuids.
+
+        Test establishing a gatt connection between a GATT server and GATT
+        client with multiple service uuid variations.
+
+        Steps:
+          1. Start a generic advertisement.
+          2. Start a generic scanner.
+          3. Find the advertisement and extract the mac address.
+          4. Stop the first scanner.
+          5. Create a GATT connection between the scanner and advertiser.
+          6. From the scanner (central device), discover services.
+          7. Verify that all the service uuid variations are found.
+          8. Disconnect the GATT connection.
+
+        Expected Result:
+          Verify that a connection was established and then disconnected
+          successfully. Verify that the service uuid variations are found.
+
+        Returns:
+          Pass if True
+          Fail if False
+
+        TAGS: LE, Advertising, Filtering, Scanning, GATT, Services
+        Priority: 2
+        """
+        try:
+            gatt_server_cb, gatt_server = setup_multiple_services(self.peripheral)
+            self.gatt_server_list.append(gatt_server)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.adv_instances.append(adv_callback)
+        if self.central.sl4a.gattClientDiscoverServices(bluetooth_gatt):
+            expected_event = GattCallbackString.GATT_SERV_DISC.format(gatt_callback)
+            try:
+                event = self.central.ed.pop_event(expected_event, self.default_timeout)
+            except Empty:
+                logging.error(GattCallbackError.GATT_SERV_DISC_ERR.format(expected_event))
+                return False
+            discovered_services_index = event['data']['ServicesIndex']
+            log_gatt_server_uuids(self.central, discovered_services_index)
+
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='7d3442c5-f71f-44ae-bd35-f2569f01b3b8')
+    def test_gatt_connect_in_quick_succession(self):
+        """Test GATT connections multiple times.
+
+        Test establishing a gatt connection between a GATT server and GATT
+        client with multiple iterations.
+
+        Steps:
+          1. Start a generic advertisement.
+          2. Start a generic scanner.
+          3. Find the advertisement and extract the mac address.
+          4. Stop the first scanner.
+          5. Create a GATT connection between the scanner and advertiser.
+          6. Disconnect the GATT connection.
+          7. Repeat steps 5 and 6 twenty times.
+
+        Expected Result:
+          Verify that a connection was established and then disconnected
+          successfully twenty times.
+
+        Returns:
+          Pass if True
+          Fail if False
+
+        TAGS: LE, Advertising, Filtering, Scanning, GATT, Stress
+        Priority: 1
+        """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        mac_address, adv_callback, scan_callback = get_mac_address_of_generic_advertisement(
+            self.central, self.peripheral)
+        autoconnect = False
+        for i in range(100):
+            logging.info("Starting connection iteration {}".format(i + 1))
+            try:
+                bluetooth_gatt, gatt_callback = setup_gatt_connection(self.central, mac_address, autoconnect)
+                self.central.sl4a.bleStopBleScan(scan_callback)
+            except GattTestUtilsError as err:
+                logging.error(err)
+                return False
+            test_result = self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+            if not test_result:
+                logging.info("Failed to disconnect from peripheral device.")
+                return False
+        self.adv_instances.append(adv_callback)
+        return True
+
+    @test_tracker_info(uuid='148469d9-7ab0-4c08-b2e9-7e49e88da1fc')
+    def test_gatt_connect_on_path_attack(self):
+        """Test GATT connection with permission write encrypted with on-path attacker prevention
+
+        Test establishing a gatt connection between a GATT server and GATT
+        client while the GATT server's characteristic includes the property
+        write value and the permission write encrypted on-path attacker prevention
+        value. This will prompt LE pairing and then the devices will create a bond.
+
+        Steps:
+          1. Create a GATT server and server callback on the peripheral device.
+          2. Create a unique service and characteristic uuid on the peripheral.
+          3. Create a characteristic on the peripheral with these properties:
+              GattCharacteristic.PROPERTY_WRITE,
+              GattCharacteristic.PERMISSION_WRITE_ENCRYPTED_MITM
+          4. Create a GATT service on the peripheral.
+          5. Add the characteristic to the GATT service.
+          6. Create a GATT connection between your central and peripheral device.
+          7. From the central device, discover the peripheral's services.
+          8. Iterate the services found until you find the unique characteristic
+              created in step 3.
+          9. Once found, write a random but valid value to the characteristic.
+          10. Start pairing helpers on both devices immediately after attempting
+              to write to the characteristic.
+          11. Within 10 seconds of writing the characteristic, there should be
+              a prompt to bond the device from the peripheral. The helpers will
+              handle the UI interaction automatically. (see
+              BluetoothConnectionFacade.java bluetoothStartPairingHelper).
+          12. Verify that the two devices are bonded.
+
+        Expected Result:
+          Verify that a connection was established and the devices are bonded.
+
+        Returns:
+          Pass if True
+          Fail if False
+
+        TAGS: LE, Advertising, Filtering, Scanning, GATT, Characteristic, OnPathAttacker
+        Priority: 1
+        """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        service_uuid = "3846D7A0-69C8-11E4-BA00-0002A5D5C51B"
+        test_uuid = "aa7edd5a-4d1d-4f0e-883a-d145616a1630"
+        bonded = False
+        characteristic = self.peripheral.sl4a.gattServerCreateBluetoothGattCharacteristic(
+            test_uuid, GattCharacteristic.PROPERTY_WRITE, GattCharacteristic.PERMISSION_WRITE_ENCRYPTED_MITM)
+        gatt_service = self.peripheral.sl4a.gattServerCreateService(service_uuid, GattServiceType.SERVICE_TYPE_PRIMARY)
+        self.peripheral.sl4a.gattServerAddCharacteristicToService(gatt_service, characteristic)
+        self.peripheral.sl4a.gattServerAddService(gatt_server, gatt_service)
+        result = self._find_service_added_event(gatt_server_cb, service_uuid)
+        if not result:
+            return False
+        bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+        self.bluetooth_gatt_list.append(bluetooth_gatt)
+        self.adv_instances.append(adv_callback)
+        if self.central.sl4a.gattClientDiscoverServices(bluetooth_gatt):
+            expected_event = GattCallbackString.GATT_SERV_DISC.format(gatt_callback)
+            try:
+                event = self.central.ed.pop_event(expected_event, self.default_timeout)
+            except Empty:
+                logging.error(GattCallbackError.GATT_SERV_DISC_ERR.format(expected_event))
+                return False
+            discovered_services_index = event['data']['ServicesIndex']
+        else:
+            logging.info("Failed to discover services.")
+            return False
+        test_value = [1, 2, 3, 4, 5, 6, 7]
+        services_count = self.central.sl4a.gattClientGetDiscoveredServicesCount(discovered_services_index)
+        for i in range(services_count):
+            characteristic_uuids = (self.central.sl4a.gattClientGetDiscoveredCharacteristicUuids(
+                discovered_services_index, i))
+            for characteristic_uuid in characteristic_uuids:
+                if characteristic_uuid == test_uuid:
+                    self.central.sl4a.bluetoothStartPairingHelper()
+                    self.peripheral.sl4a.bluetoothStartPairingHelper()
+                    self.central.sl4a.gattClientCharacteristicSetValue(bluetooth_gatt, discovered_services_index, i,
+                                                                       characteristic_uuid, test_value)
+                    self.central.sl4a.gattClientWriteCharacteristic(bluetooth_gatt, discovered_services_index, i,
+                                                                    characteristic_uuid)
+                    start_time = time.time() + self.default_timeout
+                    target_name = self.peripheral.sl4a.bluetoothGetLocalName()
+                    while time.time() < start_time and bonded == False:
+                        bonded_devices = \
+                            self.central.sl4a.bluetoothGetBondedDevices()
+                        for device in bonded_devices:
+                            if ('name' in device.keys() and device['name'] == target_name):
+                                bonded = True
+                                break
+                    bonded = False
+                    target_name = self.central.sl4a.bluetoothGetLocalName()
+                    while time.time() < start_time and bonded == False:
+                        bonded_devices = \
+                            self.peripheral.sl4a.bluetoothGetBondedDevices()
+                        for device in bonded_devices:
+                            if ('name' in device.keys() and device['name'] == target_name):
+                                bonded = True
+                                break
+
+        # Dual mode devices will establish connection over the classic transport,
+        # in order to establish bond over both transports, and do SDP. Starting
+        # disconnection before all this is finished is not safe, might lead to
+        # race conditions, i.e. bond over classic tranport shows up after LE
+        # bond is already removed.
+        time.sleep(4)
+
+        for ad in [self.central, self.peripheral]:
+            if not clear_bonded_devices(ad):
+                return False
+
+        # Necessary sleep time for entries to update unbonded state
+        time.sleep(2)
+
+        for ad in [self.central, self.peripheral]:
+            bonded_devices = ad.sl4a.bluetoothGetBondedDevices()
+            if len(bonded_devices) > 0:
+                logging.error("Failed to unbond devices: {}".format(bonded_devices))
+                return False
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='cc3fc361-7bf1-4ee2-9e46-4a27c88ce6a8')
+    def test_gatt_connect_get_connected_devices(self):
+        """Test GATT connections show up in getConnectedDevices
+
+        Test establishing a gatt connection between a GATT server and GATT
+        client. Verify that active connections show up using
+        BluetoothManager.getConnectedDevices API.
+
+        Steps:
+          1. Start a generic advertisement.
+          2. Start a generic scanner.
+          3. Find the advertisement and extract the mac address.
+          4. Stop the first scanner.
+          5. Create a GATT connection between the scanner and advertiser.
+          7. Verify the GATT Client has an open connection to the GATT Server.
+          8. Verify the GATT Server has an open connection to the GATT Client.
+          9. Disconnect the GATT connection.
+
+        Expected Result:
+          Verify that a connection was established, connected devices are found
+          on both the central and peripheral devices, and then disconnected
+          successfully.
+
+        Returns:
+          Pass if True
+          Fail if False
+
+        TAGS: LE, Advertising, Scanning, GATT
+        Priority: 2
+        """
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+        self.gatt_server_list.append(gatt_server)
+        try:
+            bluetooth_gatt, gatt_callback, adv_callback = (orchestrate_gatt_connection(self.central, self.peripheral))
+            self.bluetooth_gatt_list.append(bluetooth_gatt)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        conn_cen_devices = self.central.sl4a.bluetoothGetConnectedLeDevices(BluetoothProfile.GATT)
+        conn_per_devices = self.peripheral.sl4a.bluetoothGetConnectedLeDevices(BluetoothProfile.GATT_SERVER)
+        target_name = self.peripheral.sl4a.bluetoothGetLocalName()
+        error_message = ("Connected device {} not found in list of connected " "devices {}")
+        if not any(d['name'] == target_name for d in conn_cen_devices):
+            logging.error(error_message.format(target_name, conn_cen_devices))
+            return False
+        # For the GATT server only check the size of the list since
+        # it may or may not include the device name.
+        target_name = self.central.sl4a.bluetoothGetLocalName()
+        if not conn_per_devices:
+            logging.error(error_message.format(target_name, conn_per_devices))
+            return False
+        self.adv_instances.append(adv_callback)
+        return self._orchestrate_gatt_disconnection(bluetooth_gatt, gatt_callback)
+
+    @test_tracker_info(uuid='a0a37ca6-9fa8-4d35-9fdb-0e25b4b8a363')
+    def test_gatt_connect_second_adv_after_canceling_first_adv(self):
+        """Test GATT connection to peripherals second advertising address.
+
+        Test the ability of cancelling GATT connections and trying to reconnect
+        to the same device via a different address.
+
+        Steps:
+          1. A starts advertising
+          2. B starts scanning and finds A's mac address
+          3. Stop advertisement from step 1. Start a new advertisement on A and
+            find the new new mac address, B knows of both old and new address.
+          4. B1 sends connect request to old address of A
+          5. B1 cancel connect attempt after 10 seconds
+          6. B1 sends connect request to new address of A
+          7. Verify B1 establish connection to A in less than 10 seconds
+
+        Expected Result:
+          Verify that a connection was established only on the second
+          advertisement's mac address.
+
+        Returns:
+          Pass if True
+          Fail if False
+
+        TAGS: LE, Advertising, Scanning, GATT
+        Priority: 3
+        """
+        autoconnect = False
+        transport = GattTransport.TRANSPORT_AUTO
+        opportunistic = False
+        # Setup a basic Gatt server on the peripheral
+        gatt_server_cb = self.peripheral.sl4a.gattServerCreateGattServerCallback()
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_cb)
+
+        # Set advertisement settings to include local name in advertisement
+        # and set the advertising mode to low_latency.
+        self.peripheral.sl4a.bleSetAdvertiseSettingsIsConnectable(True)
+        self.peripheral.sl4a.bleSetAdvertiseDataIncludeDeviceName(True)
+        self.peripheral.sl4a.bleSetAdvertiseSettingsAdvertiseMode(BleAdvertiseSettingsMode.LOW_LATENCY)
+
+        # Setup necessary advertisement objects.
+        advertise_data = self.peripheral.sl4a.bleBuildAdvertiseData()
+        advertise_settings = self.peripheral.sl4a.bleBuildAdvertiseSettings()
+        advertise_callback = self.peripheral.sl4a.bleGenBleAdvertiseCallback()
+
+        # Step 1: Start advertisement
+        self.peripheral.sl4a.bleStartBleAdvertising(advertise_callback, advertise_data, advertise_settings)
+
+        # Setup scan settings for low_latency scanning and to include the local name
+        # of the advertisement started in step 1.
+        filter_list = self.central.sl4a.bleGenFilterList()
+        self.central.sl4a.bleSetScanSettingsNumOfMatches(BleScanSettingsMatchNums.ONE)
+        self.central.sl4a.bleSetScanFilterDeviceName(self.peripheral.sl4a.bluetoothGetLocalName())
+        self.central.sl4a.bleBuildScanFilter(filter_list)
+        self.central.sl4a.bleSetScanSettingsScanMode(BleScanSettingsModes.LOW_LATENCY)
+
+        # Setup necessary scan objects.
+        scan_settings = self.central.sl4a.bleBuildScanSetting()
+        scan_callback = self.central.sl4a.bleGenScanCallback()
+
+        # Step 2: Start scanning on central Android device and find peripheral
+        # address.
+        self.central.sl4a.bleStartBleScan(filter_list, scan_settings, scan_callback)
         expected_event_name = scan_result.format(scan_callback)
+        try:
+            mac_address_pre_restart = self.central.ed.pop_event(
+                expected_event_name, self.default_timeout)['data']['Result']['deviceInfo']['address']
+            logging.info("Peripheral advertisement found with mac address: {}".format(mac_address_pre_restart))
+        except Empty:
+            logging.info("Peripheral advertisement not found")
+            test_result = False
 
-        # Start scanning on SL4A DUT
-        self.dut.sl4a.bleSetScanFilterDeviceAddressTypeAndIrkHexString(cert_public_address, int(addr_type), irk)
-        self.dut.sl4a.bleBuildScanFilter(filter_list)
-        self.dut.sl4a.bleStartBleScan(filter_list, scan_settings, scan_callback)
-        logging.info("Started scanning")
+        # Step 3: Restart peripheral advertising such that a new mac address is
+        # created.
+        self.peripheral.sl4a.bleStopBleAdvertising(advertise_callback)
+        self.peripheral.sl4a.bleStartBleAdvertising(advertise_callback, advertise_data, advertise_settings)
 
-        # Verify that scan result is received on SL4A DUT
-        mac_address = self._wait_for_scan_result_event(expected_event_name, self.dut)
-        assertThat(mac_address).isNotNone()
-        logging.info("Filter advertisement with address {}".format(mac_address))
+        mac_address_post_restart = mac_address_pre_restart
 
-        # Stop scanning and try to connect GATT
-        self.dut.sl4a.bleStopBleScan(scan_callback)
-        gatt_callback = self.dut.sl4a.gattCreateGattCallback()
-        bluetooth_gatt = self.dut.sl4a.gattClientConnectGatt(gatt_callback, mac_address, False, gatt_transport['le'],
-                                                             False, None)
-        assertThat(bluetooth_gatt).isNotNone()
+        while True:
+            try:
+                mac_address_post_restart = self.central.ed.pop_event(
+                    expected_event_name, self.default_timeout)['data']['Result']['deviceInfo']['address']
+                logging.info("Peripheral advertisement found with mac address: {}".format(mac_address_post_restart))
+            except Empty:
+                logging.info("Peripheral advertisement not found")
+                test_result = False
 
-        # Verify that GATT connect event occurs on SL4A DUT
-        expected_event_name = gatt_connection_state_change.format(gatt_callback)
-        assertThat(self._wait_for_event(expected_event_name, self.dut)).isTrue()
-
-        # Test over
-        self.cert.sl4a.bleStopBleAdvertising(advertise_callback)
+            if mac_address_pre_restart != mac_address_post_restart:
+                break
 
 
 if __name__ == '__main__':
diff --git a/system/blueberry/tests/sl4a_sl4a/gatt/gatt_connect_with_irk_test.py b/system/blueberry/tests/sl4a_sl4a/gatt/gatt_connect_with_irk_test.py
new file mode 100644
index 0000000..eab98fe
--- /dev/null
+++ b/system/blueberry/tests/sl4a_sl4a/gatt/gatt_connect_with_irk_test.py
@@ -0,0 +1,148 @@
+#!/usr/bin/env python3
+#
+#   Copyright 2021 - The Android Open Source Project
+#
+#   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.
+
+import io
+import logging
+import os
+import queue
+
+from blueberry.facade import common_pb2 as common
+from blueberry.tests.gd.cert.context import get_current_context
+from blueberry.tests.gd.cert.truth import assertThat
+from blueberry.tests.gd_sl4a.lib.ble_lib import generate_ble_advertise_objects
+from blueberry.tests.gd_sl4a.lib.ble_lib import generate_ble_scan_objects
+from blueberry.tests.gd_sl4a.lib.bt_constants import adv_succ
+from blueberry.tests.gd_sl4a.lib.bt_constants import ble_address_types
+from blueberry.tests.gd_sl4a.lib.bt_constants import ble_advertise_settings_modes
+from blueberry.tests.gd_sl4a.lib.bt_constants import ble_scan_settings_modes
+from blueberry.tests.gd_sl4a.lib.bt_constants import scan_result
+from blueberry.tests.sl4a_sl4a.lib import sl4a_sl4a_base_test
+from blueberry.utils.bt_gatt_constants import GattCallbackString
+from blueberry.utils.bt_gatt_constants import GattTransport
+from mobly import test_runner
+from mobly.controllers.android_device_lib.adb import AdbError
+
+
+class GattConnectWithIrkTest(sl4a_sl4a_base_test.Sl4aSl4aBaseTestClass):
+
+    def setup_class(self):
+        super().setup_class()
+        self.default_timeout = 10  # seconds
+
+    def setup_test(self):
+        super().setup_test()
+
+    def teardown_test(self):
+        super().teardown_test()
+
+    def _wait_for_event(self, expected_event_name, device):
+        try:
+            event_info = device.ed.pop_event(expected_event_name, self.default_timeout)
+            logging.info(event_info)
+        except queue.Empty as error:
+            logging.error("Failed to find event: %s", expected_event_name)
+            return False
+        return True
+
+    def _wait_for_scan_result_event(self, expected_event_name, device):
+        try:
+            event_info = device.ed.pop_event(expected_event_name, self.default_timeout)
+        except queue.Empty as error:
+            logging.error("Could not find scan result event: %s", expected_event_name)
+            return None
+        return event_info['data']['Result']['deviceInfo']['address']
+
+    def _get_cert_public_address_and_irk_from_bt_config(self):
+        # Pull IRK from SL4A cert side to pass in from SL4A DUT side when scanning
+        bt_config_file_path = os.path.join(get_current_context().get_full_output_path(),
+                                           "DUT_%s_bt_config.conf" % self.cert.serial)
+        try:
+            self.cert.adb.pull(["/data/misc/bluedroid/bt_config.conf", bt_config_file_path])
+        except AdbError as error:
+            logging.error("Failed to pull SL4A cert BT config")
+            return False
+        logging.debug("Reading SL4A cert BT config")
+        with io.open(bt_config_file_path) as f:
+            for line in f.readlines():
+                stripped_line = line.strip()
+                if (stripped_line.startswith("Address")):
+                    address_fields = stripped_line.split(' ')
+                    # API currently requires public address to be capitalized
+                    address = address_fields[2].upper()
+                    logging.debug("Found cert address: %s" % address)
+                    continue
+                if (stripped_line.startswith("LE_LOCAL_KEY_IRK")):
+                    irk_fields = stripped_line.split(' ')
+                    irk = irk_fields[2]
+                    logging.debug("Found cert IRK: %s" % irk)
+                    continue
+
+        return address, irk
+
+    def test_scan_connect_unbonded_device_public_address_with_irk(self):
+        # Set up SL4A cert side to advertise
+        logging.info("Starting advertising")
+        self.cert.sl4a.bleSetAdvertiseSettingsIsConnectable(True)
+        self.cert.sl4a.bleSetAdvertiseDataIncludeDeviceName(True)
+        self.cert.sl4a.bleSetAdvertiseSettingsAdvertiseMode(ble_advertise_settings_modes['low_latency'])
+        self.cert.sl4a.bleSetAdvertiseSettingsOwnAddressType(common.RANDOM_DEVICE_ADDRESS)
+        advertise_callback, advertise_data, advertise_settings = generate_ble_advertise_objects(self.cert.sl4a)
+        self.cert.sl4a.bleStartBleAdvertising(advertise_callback, advertise_data, advertise_settings)
+
+        # Wait for SL4A cert to start advertising
+        assertThat(self._wait_for_event(adv_succ.format(advertise_callback), self.cert)).isTrue()
+        logging.info("Advertising started")
+
+        # Pull IRK from SL4A cert side to pass in from SL4A DUT side when scanning
+        cert_public_address, irk = self._get_cert_public_address_and_irk_from_bt_config()
+
+        # Set up SL4A DUT side to scan
+        addr_type = ble_address_types["public"]
+        logging.info("Start scanning for PUBLIC_ADDRESS %s with address type %d and IRK %s" % (cert_public_address,
+                                                                                               addr_type, irk))
+        self.dut.sl4a.bleSetScanSettingsScanMode(ble_scan_settings_modes['low_latency'])
+        self.dut.sl4a.bleSetScanSettingsLegacy(False)
+        filter_list, scan_settings, scan_callback = generate_ble_scan_objects(self.dut.sl4a)
+        expected_event_name = scan_result.format(scan_callback)
+
+        # Start scanning on SL4A DUT
+        self.dut.sl4a.bleSetScanFilterDeviceAddressTypeAndIrkHexString(cert_public_address, int(addr_type), irk)
+        self.dut.sl4a.bleBuildScanFilter(filter_list)
+        self.dut.sl4a.bleStartBleScan(filter_list, scan_settings, scan_callback)
+        logging.info("Started scanning")
+
+        # Verify that scan result is received on SL4A DUT
+        mac_address = self._wait_for_scan_result_event(expected_event_name, self.dut)
+        assertThat(mac_address).isNotNone()
+        logging.info("Filter advertisement with address {}".format(mac_address))
+
+        # Stop scanning and try to connect GATT
+        self.dut.sl4a.bleStopBleScan(scan_callback)
+        gatt_callback = self.dut.sl4a.gattCreateGattCallback()
+        bluetooth_gatt = self.dut.sl4a.gattClientConnectGatt(gatt_callback, mac_address, False,
+                                                             GattTransport.TRANSPORT_LE, False, None)
+        assertThat(bluetooth_gatt).isNotNone()
+
+        # Verify that GATT connect event occurs on SL4A DUT
+        expected_event_name = GattCallbackString.GATT_CONN_CHANGE.format(gatt_callback)
+        assertThat(self._wait_for_event(expected_event_name, self.dut)).isTrue()
+
+        # Test over
+        self.cert.sl4a.bleStopBleAdvertising(advertise_callback)
+
+
+if __name__ == '__main__':
+    test_runner.main()
diff --git a/system/blueberry/tests/sl4a_sl4a/gatt/gatt_notify_test.py b/system/blueberry/tests/sl4a_sl4a/gatt/gatt_notify_test.py
new file mode 100644
index 0000000..5e02675
--- /dev/null
+++ b/system/blueberry/tests/sl4a_sl4a/gatt/gatt_notify_test.py
@@ -0,0 +1,98 @@
+#!/usr/bin/env python3
+#
+# Copyright (C) 2016 The Android Open Source Project
+#
+# 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.
+"""
+This test script exercises GATT notify/indicate procedures.
+
+Original location:
+  tools/test/connectivity/acts_tests/tests/google/ble/gatt/GattNotifyTest.py
+"""
+
+from blueberry.tests.gd.cert.test_decorators import test_tracker_info
+
+from blueberry.tests.gd.cert.truth import assertThat
+from blueberry.tests.sl4a_sl4a.lib import gatt_connected_base_test
+from blueberry.utils.bt_gatt_constants import GattCharDesc
+from blueberry.utils.bt_gatt_constants import GattDescriptor
+from blueberry.utils.bt_gatt_constants import GattEvent
+from mobly import test_runner
+
+
+class GattNotifyTest(gatt_connected_base_test.GattConnectedBaseTest):
+
+    @test_tracker_info(uuid='e0ba60af-c1f2-4516-a5d5-89e2def0c757')
+    def test_notify_char(self):
+        """Test notify characteristic value.
+
+        Test GATT notify characteristic value.
+
+        Steps:
+        1. Central: write CCC - register for notifications.
+        2. Peripheral: receive CCC modification.
+        3. Peripheral: send characteristic notification.
+        4. Central: receive notification, verify it's conent matches what was
+           sent
+
+        Expected Result:
+        Verify that notification registration and delivery works.
+
+        Returns:
+          Pass if True
+          Fail if False
+
+        TAGS: LE, GATT, Characteristic
+        Priority: 0
+        """
+        # write CCC descriptor to enable notifications
+        self.central.sl4a.gattClientDescriptorSetValue(
+            self.bluetooth_gatt, self.discovered_services_index, self.test_service_index, self.NOTIFIABLE_CHAR_UUID,
+            GattCharDesc.GATT_CLIENT_CHARAC_CFG_UUID, GattDescriptor.ENABLE_NOTIFICATION_VALUE)
+
+        self.central.sl4a.gattClientWriteDescriptor(self.bluetooth_gatt, self.discovered_services_index,
+                                                    self.test_service_index, self.NOTIFIABLE_CHAR_UUID,
+                                                    GattCharDesc.GATT_CLIENT_CHARAC_CFG_UUID)
+
+        # enable notifications in app
+        self.central.sl4a.gattClientSetCharacteristicNotification(self.bluetooth_gatt, self.discovered_services_index,
+                                                                  self.test_service_index, self.NOTIFIABLE_CHAR_UUID,
+                                                                  True)
+
+        event = self._server_wait(GattEvent.DESC_WRITE_REQ)
+
+        request_id = event['data']['requestId']
+        bt_device_id = 0
+        status = 0
+        # confirm notification registration was successful
+        self.peripheral.sl4a.gattServerSendResponse(self.gatt_server, bt_device_id, request_id, status, 0, [])
+        # wait for client to get response
+        event = self._client_wait(GattEvent.DESC_WRITE)
+
+        # set notified value
+        notified_value = [1, 5, 9, 7, 5, 3, 6, 4, 8, 2]
+        self.peripheral.sl4a.gattServerCharacteristicSetValue(self.notifiable_char_index, notified_value)
+
+        # send notification
+        self.peripheral.sl4a.gattServerNotifyCharacteristicChanged(self.gatt_server, bt_device_id,
+                                                                   self.notifiable_char_index, False)
+
+        # wait for client to receive the notification
+        event = self._client_wait(GattEvent.CHAR_CHANGE)
+        assertThat(event["data"]["CharacteristicValue"]).isEqualTo(notified_value)
+
+        return True
+
+
+if __name__ == '__main__':
+    test_runner.main()
diff --git a/system/blueberry/tests/sl4a_sl4a/lib/gatt_connected_base_test.py b/system/blueberry/tests/sl4a_sl4a/lib/gatt_connected_base_test.py
new file mode 100644
index 0000000..9a4e606
--- /dev/null
+++ b/system/blueberry/tests/sl4a_sl4a/lib/gatt_connected_base_test.py
@@ -0,0 +1,189 @@
+#!/usr/bin/env python3
+#
+# Copyright (C) 2016 The Android Open Source Project
+#
+# 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.
+"""
+This is base class for tests that exercises different GATT procedures between two connected devices.
+Setup/Teardown methods take care of establishing connection, and doing GATT DB initialization/discovery.
+
+Original file:
+    tools/test/connectivity/acts_tests/acts_contrib/test_utils/bt/GattConnectedBaseTest.py
+"""
+
+import logging
+from queue import Empty
+
+from blueberry.tests.gd_sl4a.lib.bt_constants import bt_default_timeout
+from blueberry.tests.sl4a_sl4a.lib.sl4a_sl4a_base_test import Sl4aSl4aBaseTestClass
+from blueberry.utils.bt_gatt_constants import GattCallbackError
+from blueberry.utils.bt_gatt_constants import GattCallbackString
+from blueberry.utils.bt_gatt_constants import GattCharDesc
+from blueberry.utils.bt_gatt_constants import GattCharacteristic
+from blueberry.utils.bt_gatt_constants import GattDescriptor
+from blueberry.utils.bt_gatt_constants import GattEvent
+from blueberry.utils.bt_gatt_constants import GattMtuSize
+from blueberry.utils.bt_gatt_constants import GattServiceType
+from blueberry.utils.bt_gatt_utils import GattTestUtilsError
+from blueberry.utils.bt_gatt_utils import disconnect_gatt_connection
+from blueberry.utils.bt_gatt_utils import orchestrate_gatt_connection
+from blueberry.utils.bt_gatt_utils import setup_gatt_characteristics
+from blueberry.utils.bt_gatt_utils import setup_gatt_descriptors
+
+
+class GattConnectedBaseTest(Sl4aSl4aBaseTestClass):
+
+    TEST_SERVICE_UUID = "3846D7A0-69C8-11E4-BA00-0002A5D5C51B"
+    READABLE_CHAR_UUID = "21c0a0bf-ad51-4a2d-8124-b74003e4e8c8"
+    READABLE_DESC_UUID = "aa7edd5a-4d1d-4f0e-883a-d145616a1630"
+    WRITABLE_CHAR_UUID = "aa7edd5a-4d1d-4f0e-883a-d145616a1630"
+    WRITABLE_DESC_UUID = "76d5ed92-ca81-4edb-bb6b-9f019665fb32"
+    NOTIFIABLE_CHAR_UUID = "b2c83efa-34ca-11e6-ac61-9e71128cae77"
+
+    def setup_class(self):
+        super().setup_class()
+        self.central = self.dut
+        self.peripheral = self.cert
+
+    def setup_test(self):
+        super(GattConnectedBaseTest, self).setup_test()
+
+        self.gatt_server_callback, self.gatt_server = \
+            self._setup_multiple_services()
+        if not self.gatt_server_callback or not self.gatt_server:
+            raise AssertionError('Service setup failed')
+
+        self.bluetooth_gatt, self.gatt_callback, self.adv_callback = (orchestrate_gatt_connection(
+            self.central, self.peripheral))
+        self.peripheral.sl4a.bleStopBleAdvertising(self.adv_callback)
+
+        self.mtu = GattMtuSize.MIN
+
+        if self.central.sl4a.gattClientDiscoverServices(self.bluetooth_gatt):
+            event = self._client_wait(GattEvent.GATT_SERV_DISC)
+            self.discovered_services_index = event['data']['ServicesIndex']
+        services_count = self.central.sl4a.gattClientGetDiscoveredServicesCount(self.discovered_services_index)
+        self.test_service_index = None
+        for i in range(services_count):
+            disc_service_uuid = (self.central.sl4a.gattClientGetDiscoveredServiceUuid(
+                self.discovered_services_index, i).upper())
+            if disc_service_uuid == self.TEST_SERVICE_UUID:
+                self.test_service_index = i
+                break
+
+        if not self.test_service_index:
+            print("Service not found")
+            return False
+
+        connected_device_list = self.peripheral.sl4a.gattServerGetConnectedDevices(self.gatt_server)
+        if len(connected_device_list) == 0:
+            logging.info("No devices connected from peripheral.")
+            return False
+
+        return True
+
+    def teardown_test(self):
+        self.peripheral.sl4a.gattServerClearServices(self.gatt_server)
+        self.peripheral.sl4a.gattServerClose(self.gatt_server)
+
+        del self.gatt_server_callback
+        del self.gatt_server
+
+        self._orchestrate_gatt_disconnection(self.bluetooth_gatt, self.gatt_callback)
+
+        return super(GattConnectedBaseTest, self).teardown_test()
+
+    def _server_wait(self, gatt_event):
+        return self._timed_pop(gatt_event, self.peripheral, self.gatt_server_callback)
+
+    def _client_wait(self, gatt_event: GattEvent):
+        return self._timed_pop(gatt_event, self.central, self.gatt_callback)
+
+    def _timed_pop(self, gatt_event: GattEvent, sl4a, gatt_callback):
+        expected_event = gatt_event["evt"].format(gatt_callback)
+        try:
+            return sl4a.ed.pop_event(expected_event, bt_default_timeout)
+        except Empty as emp:
+            raise AssertionError(gatt_event["err"].format(expected_event))
+
+    def _setup_characteristics_and_descriptors(self, droid):
+        characteristic_input = [
+            {
+                'uuid': self.WRITABLE_CHAR_UUID,
+                'property': GattCharacteristic.PROPERTY_WRITE | GattCharacteristic.PROPERTY_WRITE_NO_RESPONSE,
+                'permission': GattCharacteristic.PERMISSION_WRITE
+            },
+            {
+                'uuid': self.READABLE_CHAR_UUID,
+                'property': GattCharacteristic.PROPERTY_READ,
+                'permission': GattCharacteristic.PROPERTY_READ
+            },
+            {
+                'uuid': self.NOTIFIABLE_CHAR_UUID,
+                'property': GattCharacteristic.PROPERTY_NOTIFY | GattCharacteristic.PROPERTY_INDICATE,
+                'permission': GattCharacteristic.PERMISSION_READ
+            },
+        ]
+        descriptor_input = [{
+            'uuid': self.WRITABLE_DESC_UUID,
+            'property': GattDescriptor.PERMISSION_READ[0] | GattDescriptor.PERMISSION_WRITE[0]
+        }, {
+            'uuid': self.READABLE_DESC_UUID,
+            'property': GattDescriptor.PERMISSION_READ[0] | GattDescriptor.PERMISSION_WRITE[0],
+        }, {
+            'uuid': GattCharDesc.GATT_CLIENT_CHARAC_CFG_UUID,
+            'property': GattDescriptor.PERMISSION_READ[0] | GattDescriptor.PERMISSION_WRITE[0],
+        }]
+        characteristic_list = setup_gatt_characteristics(droid, characteristic_input)
+        self.notifiable_char_index = characteristic_list[2]
+        descriptor_list = setup_gatt_descriptors(droid, descriptor_input)
+        return characteristic_list, descriptor_list
+
+    def _orchestrate_gatt_disconnection(self, bluetooth_gatt, gatt_callback):
+        logging.info("Disconnecting from peripheral device.")
+        try:
+            disconnect_gatt_connection(self.central, bluetooth_gatt, gatt_callback)
+        except GattTestUtilsError as err:
+            logging.error(err)
+            return False
+        self.central.sl4a.gattClientClose(bluetooth_gatt)
+        return True
+
+    def _find_service_added_event(self, gatt_server_callback, uuid):
+        expected_event = GattCallbackString.SERV_ADDED.format(gatt_server_callback)
+        try:
+            event = self.peripheral.sl4a.ed.pop_event(expected_event, bt_default_timeout)
+        except Empty:
+            logging.error(GattCallbackError.SERV_ADDED_ERR.format(expected_event))
+            return False
+        if event['data']['serviceUuid'].lower() != uuid.lower():
+            logging.error("Uuid mismatch. Found: {}, Expected {}.".format(event['data']['serviceUuid'], uuid))
+            return False
+        return True
+
+    def _setup_multiple_services(self):
+        gatt_server_callback = (self.peripheral.sl4a.gattServerCreateGattServerCallback())
+        gatt_server = self.peripheral.sl4a.gattServerOpenGattServer(gatt_server_callback)
+        characteristic_list, descriptor_list = (self._setup_characteristics_and_descriptors(self.peripheral.sl4a))
+        self.peripheral.sl4a.gattServerCharacteristicAddDescriptor(characteristic_list[0], descriptor_list[0])
+        self.peripheral.sl4a.gattServerCharacteristicAddDescriptor(characteristic_list[1], descriptor_list[1])
+        self.peripheral.sl4a.gattServerCharacteristicAddDescriptor(characteristic_list[2], descriptor_list[2])
+        gatt_service3 = self.peripheral.sl4a.gattServerCreateService(self.TEST_SERVICE_UUID,
+                                                                     GattServiceType.SERVICE_TYPE_PRIMARY)
+        for characteristic in characteristic_list:
+            self.peripheral.sl4a.gattServerAddCharacteristicToService(gatt_service3, characteristic)
+        self.peripheral.sl4a.gattServerAddService(gatt_server, gatt_service3)
+        result = self._find_service_added_event(gatt_server_callback, self.TEST_SERVICE_UUID)
+        if not result:
+            return False, False
+        return gatt_server_callback, gatt_server
diff --git a/system/blueberry/tests/sl4a_sl4a/lib/sl4a_sl4a_base_test.py b/system/blueberry/tests/sl4a_sl4a/lib/sl4a_sl4a_base_test.py
index 14eeaf4..86efbd2 100644
--- a/system/blueberry/tests/sl4a_sl4a/lib/sl4a_sl4a_base_test.py
+++ b/system/blueberry/tests/sl4a_sl4a/lib/sl4a_sl4a_base_test.py
@@ -14,79 +14,68 @@
 #   See the License for the specific language governing permissions and
 #   limitations under the License.
 
-import traceback
-import os
 import logging
-
+import os
+import traceback
 from functools import wraps
-from grpc import RpcError
-
-from mobly import signals
-from mobly.base_test import BaseTestClass
-from mobly.controllers.android_device_lib.adb import AdbError
-from mobly.controllers import android_device
-from mobly.controllers.android_device import MOBLY_CONTROLLER_CONFIG_NAME as ANDROID_DEVICE_CONFIG_NAME
-from mobly.controllers.android_device_lib.jsonrpc_client_base import \
-  AppRestoreConnectionError
-from mobly.controllers.android_device_lib.services.sl4a_service import Sl4aService
-import mobly.controllers.android_device_lib.sl4a_client as sl4a_client
 
 from blueberry.tests.gd.cert.context import get_current_context
-from blueberry.tests.gd_sl4a.lib.ble_lib import enable_bluetooth, disable_bluetooth, BleLib
+from blueberry.tests.gd_sl4a.lib.ble_lib import BleLib
+from blueberry.tests.gd_sl4a.lib.ble_lib import disable_bluetooth
+from blueberry.tests.gd_sl4a.lib.ble_lib import enable_bluetooth
+from blueberry.utils.mobly_sl4a_utils import setup_sl4a
+from blueberry.utils.mobly_sl4a_utils import teardown_sl4a
+from grpc import RpcError
+from mobly import signals
+from mobly.base_test import BaseTestClass
+from mobly.controllers import android_device
+from mobly.controllers.android_device import MOBLY_CONTROLLER_CONFIG_NAME as ANDROID_DEVICE_CONFIG_NAME
+from mobly.controllers.android_device_lib.adb import AdbError
 
 
 class Sl4aSl4aBaseTestClass(BaseTestClass):
 
     SUBPROCESS_WAIT_TIMEOUT_SECONDS = 10
 
-    def setup_sl4a(self, device, server_port, forwarded_port):
-        device.services.register('sl4a', Sl4aService, start_service=False)
-        sl4a_client._DEVICE_SIDE_PORT = server_port
-        sl4a_client._APP_START_WAIT_TIME = 0.5
-        try:
-            device.sl4a.start()
-        except AppRestoreConnectionError:
-            pass
-        try:
-            device.sl4a.clear_host_port()
-        except AdbError:
-            pass
-        sl4a_client._APP_START_WAIT_TIME = 2 * 60
-        device.sl4a.restore_app_connection(port=forwarded_port)
-
     def setup_class(self):
         self.log_path_base = get_current_context().get_full_output_path()
         self.verbose_mode = bool(self.user_params.get('verbose_mode', False))
 
         # Parse and construct Android device objects
         self.android_devices = self.register_controller(android_device, required=True)
-        self.cert = self.android_devices[1]
-        server_port = int(self.controller_configs[ANDROID_DEVICE_CONFIG_NAME][1]['server_port'])
-        forwarded_port = int(self.controller_configs[ANDROID_DEVICE_CONFIG_NAME][1]['forwarded_port'])
-        self.setup_sl4a(self.cert, server_port, forwarded_port)
 
+        # Setup SL4A for dut, overriding default mobly port settings
         self.dut = self.android_devices[0]
         server_port = int(self.controller_configs[ANDROID_DEVICE_CONFIG_NAME][0]['server_port'])
         forwarded_port = int(self.controller_configs[ANDROID_DEVICE_CONFIG_NAME][0]['forwarded_port'])
+        setup_sl4a(self.dut, server_port, forwarded_port)
 
-        sl4a_client._DEVICE_SIDE_PORT = server_port
-        sl4a_client._APP_START_WAIT_TIME = 0.5
-        self.setup_sl4a(self.dut, server_port, forwarded_port)
+        # Setup SL4A for cert, overriding default mobly port settings
+        self.cert = self.android_devices[1]
+        server_port = int(self.controller_configs[ANDROID_DEVICE_CONFIG_NAME][1]['server_port'])
+        forwarded_port = int(self.controller_configs[ANDROID_DEVICE_CONFIG_NAME][1]['forwarded_port'])
+        setup_sl4a(self.cert, server_port, forwarded_port)
 
         # Enable full btsnoop log
+        self.dut.adb.root()
         self.dut.adb.shell("setprop persist.bluetooth.btsnooplogmode full")
-        getprop_result = self.dut.adb.shell("getprop persist.bluetooth.btsnooplogmode") == "full"
-        if not getprop_result:
-            self.dut.log.warning("Failed to enable Bluetooth Hci Snoop Logging.")
+        getprop_result = self.dut.adb.getprop("persist.bluetooth.btsnooplogmode")
+        if getprop_result is None or ("full" not in getprop_result.lower()):
+            self.dut.log.warning("Failed to enable Bluetooth HCI Snoop Logging on DUT, mode is {}"
+                                 .format(getprop_result))
+        self.cert.adb.root()
         self.cert.adb.shell("setprop persist.bluetooth.btsnooplogmode full")
-        getprop_result = self.cert.adb.shell("getprop persist.bluetooth.btsnooplogmode") == "full"
-        if not getprop_result:
-            self.cert.log.warning("Failed to enable Bluetooth Hci Snoop Logging.")
+        getprop_result = self.cert.adb.getprop("persist.bluetooth.btsnooplogmode")
+        if getprop_result is None or ("full" not in getprop_result.lower()):
+            self.cert.log.warning("Failed to enable Bluetooth HCI Snoop Logging on CERT, mode is {}"
+                                  .format(getprop_result))
 
         self.ble = BleLib(dut=self.dut)
 
     def teardown_class(self):
-        pass
+        teardown_sl4a(self.cert)
+        teardown_sl4a(self.dut)
+        super().teardown_class()
 
     def setup_device_for_test(self, device):
         device.ed.clear_all_events()
diff --git a/system/blueberry/tests/sl4a_sl4a/sl4a_sl4a_device_config.yaml b/system/blueberry/tests/sl4a_sl4a/sl4a_sl4a_device_config.yaml
index 0c14066..e96a272 100644
--- a/system/blueberry/tests/sl4a_sl4a/sl4a_sl4a_device_config.yaml
+++ b/system/blueberry/tests/sl4a_sl4a/sl4a_sl4a_device_config.yaml
@@ -4,8 +4,6 @@
   Controllers:
     AndroidDevice:
     - label: dut
-      # Preferred client port number on the PC host side for SL4A
-      client_port: '8893'
       # Preferred server port number forwarded from Android to the host PC
       # via adb for SL4A connections
       forwarded_port: '8897'
@@ -13,8 +11,6 @@
       server_port: '8897'
       serial: 'DUT'
     - label: cert
-      # Preferred client port number on the PC host side for SL4A
-      client_port: '8895'
       # Preferred server port number forwarded from Android to the host PC
       # via adb for SL4A connections
       forwarded_port: '8899'
diff --git a/system/blueberry/tests/sl4a_sl4a/sl4a_sl4a_test_runner.py b/system/blueberry/tests/sl4a_sl4a/sl4a_sl4a_test_runner.py
index 96c4736..07641ff 100644
--- a/system/blueberry/tests/sl4a_sl4a/sl4a_sl4a_test_runner.py
+++ b/system/blueberry/tests/sl4a_sl4a/sl4a_sl4a_test_runner.py
@@ -15,11 +15,17 @@
 #   limitations under the License.
 
 from blueberry.tests.sl4a_sl4a.gatt.gatt_connect_test import GattConnectTest
+from blueberry.tests.sl4a_sl4a.gatt.gatt_connect_with_irk_test import GattConnectWithIrkTest
+from blueberry.tests.sl4a_sl4a.gatt.gatt_notify_test import GattNotifyTest
 
 from mobly import suite_runner
 import argparse
 
-ALL_TESTS = [GattConnectTest]
+ALL_TESTS = [
+    GattConnectTest,
+    GattConnectWithIrk,
+    GattNotifyTest,
+]
 
 
 def main():
diff --git a/system/blueberry/utils/android_bluetooth_decorator.py b/system/blueberry/utils/android_bluetooth_decorator.py
index b258d6b..e3c18d3 100644
--- a/system/blueberry/utils/android_bluetooth_decorator.py
+++ b/system/blueberry/utils/android_bluetooth_decorator.py
@@ -24,9 +24,9 @@
 
 from blueberry.controllers import derived_bt_device
 from blueberry.utils import bt_constants
+from blueberry.utils import ble_scan_adv_constants
 from blueberry.utils import bt_test_utils
 
-
 # Map for media passthrough commands and the corresponding events.
 MEDIA_CMD_MAP = {
     bt_constants.CMD_MEDIA_PAUSE: bt_constants.EVENT_PAUSE_RECEIVED,
@@ -65,70 +65,64 @@
 
 
 class DeviceBootError(signals.ControllerError):
-  """Exception raised for Android device boot failures."""
-  pass
+    """Exception raised for Android device boot failures."""
+    pass
 
 
 class Error(Exception):
-  """Raised when an operation in this module fails."""
-  pass
+    """Raised when an operation in this module fails."""
+    pass
 
 
 class DiscoveryError(signals.ControllerError):
-  """Exception raised for Bluetooth device discovery failures."""
-  pass
+    """Exception raised for Bluetooth device discovery failures."""
+    pass
 
 
 class AndroidBluetoothDecorator(android_device.AndroidDevice):
-  """Decorates an AndroidDevice with Bluetooth-specific functionality."""
+    """Decorates an AndroidDevice with Bluetooth-specific functionality."""
 
-  def __init__(self, ad: android_device.AndroidDevice):
-    self._ad = ad
-    self._user_params = None
-    if not self._ad or not isinstance(self._ad, android_device.AndroidDevice):
-      raise TypeError('Must apply AndroidBluetoothDecorator to an '
-                      'AndroidDevice')
-    self.ble_advertise_callback = None
-    self.regex_logcat_time = re.compile(
-        r'(?P<datetime>[\d]{2}-[\d]{2} [\d]{2}:[\d]{2}:[\d]{2}.[\d]{3})'
-        r'[ ]+\d+.*')
-    self._regex_bt_crash = re.compile(
-        r'Bluetooth crashed (?P<num_bt_crashes>\d+) times')
+    def __init__(self, ad: android_device.AndroidDevice):
+        self._ad = ad
+        self._user_params = None
+        if not self._ad or not isinstance(self._ad, android_device.AndroidDevice):
+            raise TypeError('Must apply AndroidBluetoothDecorator to an ' 'AndroidDevice')
+        self.ble_advertise_callback = None
+        self.regex_logcat_time = re.compile(r'(?P<datetime>[\d]{2}-[\d]{2} [\d]{2}:[\d]{2}:[\d]{2}.[\d]{3})'
+                                            r'[ ]+\d+.*')
+        self._regex_bt_crash = re.compile(r'Bluetooth crashed (?P<num_bt_crashes>\d+) times')
 
-  def __getattr__(self, name: Any) -> Any:
-    return getattr(self._ad, name)
+    def __getattr__(self, name: Any) -> Any:
+        return getattr(self._ad, name)
 
-  def _is_device_connected(self, mac_address):
-    """Wrapper method to help with unit testability of this class."""
-    return self._ad.sl4a.bluetoothIsDeviceConnected(mac_address)
+    def _is_device_connected(self, mac_address):
+        """Wrapper method to help with unit testability of this class."""
+        return self._ad.sl4a.bluetoothIsDeviceConnected(mac_address)
 
-  def _is_profile_connected(self, mac_address, profile):
-    """Checks if the profile is connected."""
-    status = None
-    pri_ad = self._ad
-    if profile == bt_constants.BluetoothProfile.HEADSET_CLIENT:
-      status = pri_ad.sl4a.bluetoothHfpClientGetConnectionStatus(mac_address)
-    elif profile == bt_constants.BluetoothProfile.A2DP_SINK:
-      status = pri_ad.sl4a.bluetoothA2dpSinkGetConnectionStatus(mac_address)
-    elif profile == bt_constants.BluetoothProfile.PBAP_CLIENT:
-      status = pri_ad.sl4a.bluetoothPbapClientGetConnectionStatus(mac_address)
-    elif profile == bt_constants.BluetoothProfile.MAP_MCE:
-      connected_devices = self._ad.sl4a.bluetoothMapClientGetConnectedDevices()
-      return any(
-          mac_address in device['address'] for device in connected_devices)
-    else:
-      pri_ad.log.warning(
-          'The connection check for profile %s is not supported '
-          'yet', profile)
-      return False
-    return status == bt_constants.BluetoothConnectionStatus.STATE_CONNECTED
+    def _is_profile_connected(self, mac_address, profile):
+        """Checks if the profile is connected."""
+        status = None
+        pri_ad = self._ad
+        if profile == bt_constants.BluetoothProfile.HEADSET_CLIENT:
+            status = pri_ad.sl4a.bluetoothHfpClientGetConnectionStatus(mac_address)
+        elif profile == bt_constants.BluetoothProfile.A2DP_SINK:
+            status = pri_ad.sl4a.bluetoothA2dpSinkGetConnectionStatus(mac_address)
+        elif profile == bt_constants.BluetoothProfile.PBAP_CLIENT:
+            status = pri_ad.sl4a.bluetoothPbapClientGetConnectionStatus(mac_address)
+        elif profile == bt_constants.BluetoothProfile.MAP_MCE:
+            connected_devices = self._ad.sl4a.bluetoothMapClientGetConnectedDevices()
+            return any(mac_address in device['address'] for device in connected_devices)
+        else:
+            pri_ad.log.warning('The connection check for profile %s is not supported ' 'yet', profile)
+            return False
+        return status == bt_constants.BluetoothConnectionStatus.STATE_CONNECTED
 
-  def _get_bluetooth_le_state(self):
-    """Wrapper method to help with unit testability of this class."""
-    return self._ad.sl4a.bluetoothGetLeState
+    def _get_bluetooth_le_state(self):
+        """Wrapper method to help with unit testability of this class."""
+        return self._ad.sl4a.bluetoothGetLeState
 
-  def _generate_id_by_size(self, size):
-    """Generate string of random ascii letters and digits.
+    def _generate_id_by_size(self, size):
+        """Generate string of random ascii letters and digits.
 
     Args:
       size: required size of string.
@@ -136,15 +130,10 @@
     Returns:
       String of random chars.
     """
-    return ''.join(
-        random.choice(string.ascii_letters + string.digits)
-        for _ in range(size))
+        return ''.join(random.choice(string.ascii_letters + string.digits) for _ in range(size))
 
-  def _wait_for_bluetooth_manager_state(self,
-                                        state=None,
-                                        timeout=10,
-                                        threshold=5):
-    """Waits for Bluetooth normalized state or normalized explicit state.
+    def _wait_for_bluetooth_manager_state(self, state=None, timeout=10, threshold=5):
+        """Waits for Bluetooth normalized state or normalized explicit state.
 
     Args:
       state: expected Bluetooth state
@@ -153,76 +142,75 @@
     Returns:
       True if successful, false if unsuccessful.
     """
-    all_states = []
-    start_time = time.time()
-    while time.time() < start_time + timeout:
-      all_states.append(self._get_bluetooth_le_state())
-      if len(all_states) >= threshold:
-        # for any normalized state
-        if state is None:
-          if len(all_states[-threshold:]) == 1:
-            logging.info('State normalized %s', all_states[-threshold:])
-            return True
-        else:
-          # explicit check against normalized state
-          if state in all_states[-threshold:]:
-            return True
-      time.sleep(0.5)
-    logging.error(
-        'Bluetooth state fails to normalize' if state is None else
-        'Failed to match bluetooth state, current state {} expected state {}'
-        .format(self._get_bluetooth_le_state(), state))
-    return False
+        all_states = []
+        start_time = time.time()
+        while time.time() < start_time + timeout:
+            all_states.append(self._get_bluetooth_le_state())
+            if len(all_states) >= threshold:
+                # for any normalized state
+                if state is None:
+                    if len(all_states[-threshold:]) == 1:
+                        logging.info('State normalized %s', all_states[-threshold:])
+                        return True
+                else:
+                    # explicit check against normalized state
+                    if state in all_states[-threshold:]:
+                        return True
+            time.sleep(0.5)
+        logging.error('Bluetooth state fails to normalize'
+                      if state is None else 'Failed to match bluetooth state, current state {} expected state {}'
+                      .format(self._get_bluetooth_le_state(), state))
+        return False
 
-  def init_setup(self) -> None:
-    """Sets up android device for bluetooth tests."""
-    self._ad.services.register('sl4a', sl4a_service.Sl4aService)
-    self._ad.load_snippet('mbs', 'com.google.android.mobly.snippet.bundled')
-    self._ad.adb.shell('setenforce 0')
+    def init_setup(self) -> None:
+        """Sets up android device for bluetooth tests."""
+        self._ad.services.register('sl4a', sl4a_service.Sl4aService)
+        self._ad.load_snippet('mbs', 'com.google.android.mobly.snippet.bundled')
+        self._ad.adb.shell('setenforce 0')
 
-    # Adds 2 seconds waiting time to see it can fix the NullPointerException
-    # when executing the following sl4a.bluetoothStartPairingHelper method.
-    time.sleep(2)
-    self._ad.sl4a.bluetoothStartPairingHelper()
-    self.factory_reset_bluetooth()
+        # Adds 2 seconds waiting time to see it can fix the NullPointerException
+        # when executing the following sl4a.bluetoothStartPairingHelper method.
+        time.sleep(2)
+        self._ad.sl4a.bluetoothStartPairingHelper()
+        self.factory_reset_bluetooth()
 
-  def sl4a_setup(self) -> None:
-    """A common setup routine for android device sl4a function.
+    def sl4a_setup(self) -> None:
+        """A common setup routine for android device sl4a function.
 
     Things this method setup:
     1. Set Bluetooth local name to random string of size 4
     2. Disable BLE background scanning.
     """
 
-    sl4a = self._ad.sl4a
-    sl4a.bluetoothStartConnectionStateChangeMonitor('')
-    setup_result = sl4a.bluetoothSetLocalName(self._generate_id_by_size(4))
-    if not setup_result:
-      self.log.error('Failed to set device name.')
-      return
-    sl4a.bluetoothDisableBLE()
-    bonded_devices = sl4a.bluetoothGetBondedDevices()
-    for b in bonded_devices:
-      self.log.info('Removing bond for device {}'.format(b['address']))
-      sl4a.bluetoothUnbond(b['address'])
+        sl4a = self._ad.sl4a
+        sl4a.bluetoothStartConnectionStateChangeMonitor('')
+        setup_result = sl4a.bluetoothSetLocalName(self._generate_id_by_size(4))
+        if not setup_result:
+            self.log.error('Failed to set device name.')
+            return
+        sl4a.bluetoothDisableBLE()
+        bonded_devices = sl4a.bluetoothGetBondedDevices()
+        for b in bonded_devices:
+            self.log.info('Removing bond for device {}'.format(b['address']))
+            sl4a.bluetoothUnbond(b['address'])
 
-  def set_user_params(self, params: Dict[str, Any]) -> None:
-    self._user_params = params
+    def set_user_params(self, params: Dict[str, Any]) -> None:
+        self._user_params = params
 
-  def get_user_params(self) -> Dict[str, Any]:
-    return self._user_params
+    def get_user_params(self) -> Dict[str, Any]:
+        return self._user_params
 
-  def is_sim_state_loaded(self) -> bool:
-    """Checks if SIM state is loaded.
+    def is_sim_state_loaded(self) -> bool:
+        """Checks if SIM state is loaded.
 
     Returns:
       True if SIM state is loaded else False.
     """
-    state = self._ad.adb.shell('getprop gsm.sim.state').decode().strip()
-    return state == 'LOADED'
+        state = self._ad.adb.shell('getprop gsm.sim.state').decode().strip()
+        return state == 'LOADED'
 
-  def is_package_installed(self, package_name: str) -> bool:
-    """Checks if a package is installed.
+    def is_package_installed(self, package_name: str) -> bool:
+        """Checks if a package is installed.
 
     Args:
       package_name: string, a package to be checked.
@@ -230,13 +218,12 @@
     Returns:
       True if the package is installed else False.
     """
-    # The package is installed if result is 1, not installed if result is 0.
-    result = int(self._ad.adb.shell('pm list packages | grep -i %s$ | wc -l' %
-                                    package_name))
-    return bool(result)
+        # The package is installed if result is 1, not installed if result is 0.
+        result = int(self._ad.adb.shell('pm list packages | grep -i %s$ | wc -l' % package_name))
+        return bool(result)
 
-  def connect_with_rfcomm(self, other_ad: android_device.AndroidDevice) -> bool:
-    """Establishes an RFCOMM connection with other android device.
+    def connect_with_rfcomm(self, other_ad: android_device.AndroidDevice) -> bool:
+        """Establishes an RFCOMM connection with other android device.
 
     Connects this android device (as a client) to the other android device
     (as a server).
@@ -247,22 +234,21 @@
     Returns:
         True if connection was successful, False if unsuccessful.
     """
-    server_address = other_ad.sl4a.bluetoothGetLocalAddress()
-    logging.info('Pairing and connecting devices')
-    if not self._ad.sl4a.bluetoothDiscoverAndBond(server_address):
-      logging.info('Failed to pair and connect devices')
-      return False
+        server_address = other_ad.sl4a.bluetoothGetLocalAddress()
+        logging.info('Pairing and connecting devices')
+        if not self._ad.sl4a.bluetoothDiscoverAndBond(server_address):
+            logging.info('Failed to pair and connect devices')
+            return False
 
-    # Create RFCOMM connection
-    logging.info('establishing RFCOMM connection')
-    return self.orchestrate_rfcomm_connection(other_ad)
+        # Create RFCOMM connection
+        logging.info('establishing RFCOMM connection')
+        return self.orchestrate_rfcomm_connection(other_ad)
 
-  def orchestrate_rfcomm_connection(
-      self,
-      other_ad: android_device.AndroidDevice,
-      accept_timeout_ms: int = bt_constants.DEFAULT_RFCOMM_TIMEOUT_MS,
-      uuid: Optional[Text] = None) -> bool:
-    """Sets up the RFCOMM connection to another android device.
+    def orchestrate_rfcomm_connection(self,
+                                      other_ad: android_device.AndroidDevice,
+                                      accept_timeout_ms: int = bt_constants.DEFAULT_RFCOMM_TIMEOUT_MS,
+                                      uuid: Optional[Text] = None) -> bool:
+        """Sets up the RFCOMM connection to another android device.
 
     It sets up the connection with a Bluetooth Socket connection with other
     device.
@@ -275,39 +261,33 @@
     Returns:
         True if connection was successful, False if unsuccessful.
     """
-    if uuid is None:
-      uuid = bt_constants.BT_RFCOMM_UUIDS['default_uuid']
-    other_ad.sl4a.bluetoothStartPairingHelper()
-    self._ad.sl4a.bluetoothStartPairingHelper()
-    other_ad.sl4a.bluetoothSocketConnBeginAcceptThreadUuid(uuid,
-                                                           accept_timeout_ms)
-    self._ad.sl4a.bluetoothSocketConnBeginConnectThreadUuid(
-        other_ad.sl4a.bluetoothGetLocalAddress(), uuid)
+        if uuid is None:
+            uuid = bt_constants.BT_RFCOMM_UUIDS['default_uuid']
+        other_ad.sl4a.bluetoothStartPairingHelper()
+        self._ad.sl4a.bluetoothStartPairingHelper()
+        other_ad.sl4a.bluetoothSocketConnBeginAcceptThreadUuid(uuid, accept_timeout_ms)
+        self._ad.sl4a.bluetoothSocketConnBeginConnectThreadUuid(other_ad.sl4a.bluetoothGetLocalAddress(), uuid)
 
-    end_time = time.time() + bt_constants.BT_DEFAULT_TIMEOUT_SECONDS
-    test_result = True
-
-    while time.time() < end_time:
-      number_socket_connections = len(
-          other_ad.sl4a.bluetoothSocketConnActiveConnections())
-      connected = number_socket_connections > 0
-      if connected:
+        end_time = time.time() + bt_constants.BT_DEFAULT_TIMEOUT_SECONDS
         test_result = True
-        other_ad.log.info('Bluetooth socket Client Connection Active')
-        break
-      else:
-        test_result = False
-      time.sleep(1)
-    if not test_result:
-      other_ad.log.error('Failed to establish a Bluetooth socket connection')
-      return False
-    return True
 
-  def wait_for_discovery_success(
-      self,
-      mac_address: str,
-      timeout: float = 30) -> float:
-    """Waits for a device to be discovered by AndroidDevice.
+        while time.time() < end_time:
+            number_socket_connections = len(other_ad.sl4a.bluetoothSocketConnActiveConnections())
+            connected = number_socket_connections > 0
+            if connected:
+                test_result = True
+                other_ad.log.info('Bluetooth socket Client Connection Active')
+                break
+            else:
+                test_result = False
+            time.sleep(1)
+        if not test_result:
+            other_ad.log.error('Failed to establish a Bluetooth socket connection')
+            return False
+        return True
+
+    def wait_for_discovery_success(self, mac_address: str, timeout: float = 30) -> float:
+        """Waits for a device to be discovered by AndroidDevice.
 
     Args:
       mac_address: The Bluetooth mac address of the peripheral device.
@@ -319,30 +299,23 @@
     Raises:
       DiscoveryError
     """
-    device_start_time = self.get_device_time()
-    start_time = time.time()
-    event_name = f'Discovery{mac_address}'
-    try:
-      self._ad.ed.wait_for_event(event_name,
-                                 lambda x: x['data']['Status'], timeout)
-      discovery_time = time.time() - start_time
-      return discovery_time
+        device_start_time = self.get_device_time()
+        start_time = time.time()
+        event_name = f'Discovery{mac_address}'
+        try:
+            self._ad.ed.wait_for_event(event_name, lambda x: x['data']['Status'], timeout)
+            discovery_time = time.time() - start_time
+            return discovery_time
 
-    except queue.Empty:
-      # TODO(user): Remove this check when this bug is fixed.
-      if self.logcat_filter(device_start_time, event_name):
-        self._ad.log.info(
-            'Actually the event "%s" was posted within %d seconds.',
-            event_name, timeout)
-        return timeout
-      raise DiscoveryError('Failed to discover device %s after %d seconds' %
-                           (mac_address, timeout))
+        except queue.Empty:
+            # TODO(user): Remove this check when this bug is fixed.
+            if self.logcat_filter(device_start_time, event_name):
+                self._ad.log.info('Actually the event "%s" was posted within %d seconds.', event_name, timeout)
+                return timeout
+            raise DiscoveryError('Failed to discover device %s after %d seconds' % (mac_address, timeout))
 
-  def wait_for_pairing_success(
-      self,
-      mac_address: str,
-      timeout: float = 30) -> float:
-    """Waits for a device to pair with the AndroidDevice.
+    def wait_for_pairing_success(self, mac_address: str, timeout: float = 30) -> float:
+        """Waits for a device to pair with the AndroidDevice.
 
     Args:
       mac_address: The Bluetooth mac address of the peripheral device.
@@ -354,23 +327,17 @@
     Raises:
       ControllerError
     """
-    start_time = time.time()
-    try:
-      self._ad.ed.wait_for_event('Bond%s' % mac_address,
-                                 lambda x: x['data']['Status'], timeout)
-      pairing_time = time.time() - start_time
-      return pairing_time
+        start_time = time.time()
+        try:
+            self._ad.ed.wait_for_event('Bond%s' % mac_address, lambda x: x['data']['Status'], timeout)
+            pairing_time = time.time() - start_time
+            return pairing_time
 
-    except queue.Empty:
-      raise signals.ControllerError(
-          'Failed to bond with device %s after %d seconds' %
-          (mac_address, timeout))
+        except queue.Empty:
+            raise signals.ControllerError('Failed to bond with device %s after %d seconds' % (mac_address, timeout))
 
-  def wait_for_connection_success(
-      self,
-      mac_address: str,
-      timeout: int = 30) -> float:
-    """Waits for a device to connect with the AndroidDevice.
+    def wait_for_connection_success(self, mac_address: str, timeout: int = 30) -> float:
+        """Waits for a device to connect with the AndroidDevice.
 
     Args:
       mac_address: The Bluetooth mac address of the peripheral device.
@@ -382,62 +349,50 @@
     Raises:
       ControllerError
     """
-    start_time = time.time()
-    end_time = start_time + timeout
-    while time.time() < end_time:
-      if self._is_device_connected(mac_address):
-        connection_time = (time.time() - start_time)
-        logging.info('Connected device %s in %d seconds', mac_address,
-                     connection_time)
-        return connection_time
+        start_time = time.time()
+        end_time = start_time + timeout
+        while time.time() < end_time:
+            if self._is_device_connected(mac_address):
+                connection_time = (time.time() - start_time)
+                logging.info('Connected device %s in %d seconds', mac_address, connection_time)
+                return connection_time
 
-    raise signals.ControllerError(
-        'Failed to connect device within %d seconds.' % timeout)
+        raise signals.ControllerError('Failed to connect device within %d seconds.' % timeout)
 
-  def factory_reset_bluetooth(self) -> None:
-    """Factory resets Bluetooth on an AndroidDevice."""
+    def factory_reset_bluetooth(self) -> None:
+        """Factory resets Bluetooth on an AndroidDevice."""
 
-    logging.info('Factory resetting Bluetooth for AndroidDevice.')
-    self._ad.sl4a.bluetoothToggleState(True)
-    paired_devices = self._ad.mbs.btGetPairedDevices()
-    for device in paired_devices:
-      self._ad.sl4a.bluetoothUnbond(device['Address'])
-    self._ad.sl4a.bluetoothFactoryReset()
-    self._wait_for_bluetooth_manager_state()
-    self.wait_for_bluetooth_toggle_state(True)
+        logging.info('Factory resetting Bluetooth for AndroidDevice.')
+        self._ad.sl4a.bluetoothToggleState(True)
+        paired_devices = self._ad.mbs.btGetPairedDevices()
+        for device in paired_devices:
+            self._ad.sl4a.bluetoothUnbond(device['Address'])
+        self._ad.sl4a.bluetoothFactoryReset()
+        self._wait_for_bluetooth_manager_state()
+        self.wait_for_bluetooth_toggle_state(True)
 
-  def get_device_info(self) -> Dict[str, Any]:
-    """Gets the configuration info of an AndroidDevice.
+    def get_device_info(self) -> Dict[str, Any]:
+        """Gets the configuration info of an AndroidDevice.
 
     Returns:
       dict, A dictionary mapping metric keys to their respective values.
     """
 
-    device_info = {
-        'device_class':
-            'AndroidDevice',
-        'device_model':
-            self._ad.device_info['model'],
-        'hardware_version':
-            self._ad.adb.getprop('ro.boot.hardware.revision'),
-        'software_version':
-            self._ad.build_info['build_id'],
-        'android_build_type':
-            self._ad.build_info['build_type'],
-        'android_build_number':
-            self._ad.adb.getprop('ro.build.version.incremental'),
-        'android_release_id':
-            self._ad.build_info['build_id']
-    }
+        device_info = {
+            'device_class': 'AndroidDevice',
+            'device_model': self._ad.device_info['model'],
+            'hardware_version': self._ad.adb.getprop('ro.boot.hardware.revision'),
+            'software_version': self._ad.build_info['build_id'],
+            'android_build_type': self._ad.build_info['build_type'],
+            'android_build_number': self._ad.adb.getprop('ro.build.version.incremental'),
+            'android_release_id': self._ad.build_info['build_id']
+        }
 
-    return device_info
+        return device_info
 
-  def pair_and_connect_bluetooth(
-      self,
-      mac_address: str,
-      attempts: int = 3,
-      enable_pairing_retry: bool = True) -> Tuple[float, float]:
-    """Pairs and connects an AndroidDevice with a peripheral Bluetooth device.
+    def pair_and_connect_bluetooth(self, mac_address: str, attempts: int = 3,
+                                   enable_pairing_retry: bool = True) -> Tuple[float, float]:
+        """Pairs and connects an AndroidDevice with a peripheral Bluetooth device.
 
     Ensures that an AndroidDevice is paired and connected to a peripheral
     device. If the devices are already connected, does nothing. If
@@ -466,50 +421,43 @@
       ControllerError: Raised if failed to bond the peripheral device.
     """
 
-    connected = self._is_device_connected(mac_address)
-    pairing_time = 0
-    connection_time = 0
-    if connected:
-      logging.info('Device %s already paired and connected', mac_address)
-      return pairing_time, connection_time
+        connected = self._is_device_connected(mac_address)
+        pairing_time = 0
+        connection_time = 0
+        if connected:
+            logging.info('Device %s already paired and connected', mac_address)
+            return pairing_time, connection_time
 
-    paired_devices = [device['address'] for device in
-                      self._ad.sl4a.bluetoothGetBondedDevices()]
-    if mac_address in paired_devices:
-      self._ad.sl4a.bluetoothConnectBonded(mac_address)
-      return pairing_time, self.wait_for_connection_success(mac_address)
+        paired_devices = [device['address'] for device in self._ad.sl4a.bluetoothGetBondedDevices()]
+        if mac_address in paired_devices:
+            self._ad.sl4a.bluetoothConnectBonded(mac_address)
+            return pairing_time, self.wait_for_connection_success(mac_address)
 
-    logging.info('Initiate pairing to the device "%s".', mac_address)
-    for i in range(attempts):
-      self._ad.sl4a.bluetoothDiscoverAndBond(mac_address)
-      try:
-        self.wait_for_discovery_success(mac_address)
-        pairing_time = self.wait_for_pairing_success(mac_address)
-        break
-      except DiscoveryError:
-        if i + 1 < attempts:
-          logging.error(
-              'Failed to find the device "%s" on Attempt %d. '
-              'Retrying discovery...', mac_address, i + 1)
-          continue
-        raise DiscoveryError('Failed to find the device "%s".' % mac_address)
-      except signals.ControllerError:
-        if i + 1 < attempts and enable_pairing_retry:
-          logging.error(
-              'Failed to bond the device "%s" on Attempt %d. '
-              'Retrying pairing...', mac_address, i + 1)
-          continue
-        raise signals.ControllerError('Failed to bond the device "%s".' %
-                                      mac_address)
+        logging.info('Initiate pairing to the device "%s".', mac_address)
+        for i in range(attempts):
+            self._ad.sl4a.bluetoothDiscoverAndBond(mac_address)
+            try:
+                self.wait_for_discovery_success(mac_address)
+                pairing_time = self.wait_for_pairing_success(mac_address)
+                break
+            except DiscoveryError:
+                if i + 1 < attempts:
+                    logging.error('Failed to find the device "%s" on Attempt %d. '
+                                  'Retrying discovery...', mac_address, i + 1)
+                    continue
+                raise DiscoveryError('Failed to find the device "%s".' % mac_address)
+            except signals.ControllerError:
+                if i + 1 < attempts and enable_pairing_retry:
+                    logging.error('Failed to bond the device "%s" on Attempt %d. '
+                                  'Retrying pairing...', mac_address, i + 1)
+                    continue
+                raise signals.ControllerError('Failed to bond the device "%s".' % mac_address)
 
-    connection_time = self.wait_for_connection_success(mac_address)
-    return pairing_time, connection_time
+        connection_time = self.wait_for_connection_success(mac_address)
+        return pairing_time, connection_time
 
-  def disconnect_bluetooth(
-      self,
-      mac_address: str,
-      timeout: float = 30) -> float:
-    """Disconnects Bluetooth between an AndroidDevice and peripheral device.
+    def disconnect_bluetooth(self, mac_address: str, timeout: float = 30) -> float:
+        """Disconnects Bluetooth between an AndroidDevice and peripheral device.
 
     Args:
       mac_address: The Bluetooth mac address of the peripheral device.
@@ -523,24 +471,23 @@
     Raises:
       ControllerError: Raised if failed to disconnect the peripheral device.
     """
-    if not self._is_device_connected(mac_address):
-      logging.info('Device %s already disconnected', mac_address)
-      return 0
+        if not self._is_device_connected(mac_address):
+            logging.info('Device %s already disconnected', mac_address)
+            return 0
 
-    self._ad.sl4a.bluetoothDisconnectConnected(mac_address)
-    start_time = time.time()
-    end_time = time.time() + timeout
-    while time.time() < end_time:
-      connected = self._is_device_connected(mac_address)
-      if not connected:
-        logging.info('Device %s disconnected successfully.', mac_address)
-        return time.time() - start_time
+        self._ad.sl4a.bluetoothDisconnectConnected(mac_address)
+        start_time = time.time()
+        end_time = time.time() + timeout
+        while time.time() < end_time:
+            connected = self._is_device_connected(mac_address)
+            if not connected:
+                logging.info('Device %s disconnected successfully.', mac_address)
+                return time.time() - start_time
 
-    raise signals.ControllerError(
-        'Failed to disconnect device within %d seconds.' % timeout)
+        raise signals.ControllerError('Failed to disconnect device within %d seconds.' % timeout)
 
-  def connect_bluetooth(self, mac_address: str, timeout: float = 30) -> float:
-    """Connects Bluetooth between an AndroidDevice and peripheral device.
+    def connect_bluetooth(self, mac_address: str, timeout: float = 30) -> float:
+        """Connects Bluetooth between an AndroidDevice and peripheral device.
 
     Args:
       mac_address: The Bluetooth mac address of the peripheral device.
@@ -554,59 +501,53 @@
     Raises:
       ControllerError: Raised if failed to connect the peripheral device.
     """
-    if self._is_device_connected(mac_address):
-      logging.info('Device %s already connected', mac_address)
-      return 0
+        if self._is_device_connected(mac_address):
+            logging.info('Device %s already connected', mac_address)
+            return 0
 
-    self._ad.sl4a.bluetoothConnectBonded(mac_address)
-    connect_time = self.wait_for_connection_success(mac_address)
+        self._ad.sl4a.bluetoothConnectBonded(mac_address)
+        connect_time = self.wait_for_connection_success(mac_address)
 
-    return connect_time
+        return connect_time
 
-  def activate_pairing_mode(self) -> None:
-    """Activates pairing mode on an AndroidDevice."""
-    logging.info('Activating pairing mode on AndroidDevice.')
-    self._ad.sl4a.bluetoothMakeDiscoverable()
-    self._ad.sl4a.bluetoothStartPairingHelper()
+    def activate_pairing_mode(self) -> None:
+        """Activates pairing mode on an AndroidDevice."""
+        logging.info('Activating pairing mode on AndroidDevice.')
+        self._ad.sl4a.bluetoothMakeDiscoverable()
+        self._ad.sl4a.bluetoothStartPairingHelper()
 
-  def activate_ble_pairing_mode(self) -> None:
-    """Activates BLE pairing mode on an AndroidDevice."""
-    self.ble_advertise_callback = self._ad.sl4a.bleGenBleAdvertiseCallback()
-    self._ad.sl4a.bleSetAdvertiseDataIncludeDeviceName(True)
-    # Sets advertise mode to low latency.
-    self._ad.sl4a.bleSetAdvertiseSettingsAdvertiseMode(
-        bt_constants.BleAdvertiseSettingsMode.LOW_LATENCY)
-    self._ad.sl4a.bleSetAdvertiseSettingsIsConnectable(True)
-    # Sets TX power level to High.
-    self._ad.sl4a.bleSetAdvertiseSettingsTxPowerLevel(
-        bt_constants.BleAdvertiseSettingsTxPower.HIGH)
-    advertise_data = self._ad.sl4a.bleBuildAdvertiseData()
-    advertise_settings = self._ad.sl4a.bleBuildAdvertiseSettings()
-    logging.info('Activating BLE pairing mode on AndroidDevice.')
-    self._ad.sl4a.bleStartBleAdvertising(
-        self.ble_advertise_callback, advertise_data, advertise_settings)
+    def activate_ble_pairing_mode(self) -> None:
+        """Activates BLE pairing mode on an AndroidDevice."""
+        self.ble_advertise_callback = self._ad.sl4a.bleGenBleAdvertiseCallback()
+        self._ad.sl4a.bleSetAdvertiseDataIncludeDeviceName(True)
+        # Sets advertise mode to low latency.
+        self._ad.sl4a.bleSetAdvertiseSettingsAdvertiseMode(ble_scan_adv_constants.BleAdvertiseSettingsMode.LOW_LATENCY)
+        self._ad.sl4a.bleSetAdvertiseSettingsIsConnectable(True)
+        # Sets TX power level to High.
+        self._ad.sl4a.bleSetAdvertiseSettingsTxPowerLevel(ble_scan_adv_constants.BleAdvertiseSettingsTxPower.HIGH)
+        advertise_data = self._ad.sl4a.bleBuildAdvertiseData()
+        advertise_settings = self._ad.sl4a.bleBuildAdvertiseSettings()
+        logging.info('Activating BLE pairing mode on AndroidDevice.')
+        self._ad.sl4a.bleStartBleAdvertising(self.ble_advertise_callback, advertise_data, advertise_settings)
 
-  def deactivate_ble_pairing_mode(self) -> None:
-    """Deactivates BLE pairing mode on an AndroidDevice."""
-    if not self.ble_advertise_callback:
-      self._ad.log.debug('BLE pairing mode is not activated.')
-      return
-    logging.info('Deactivating BLE pairing mode on AndroidDevice.')
-    self._ad.sl4a.bleStopBleAdvertising(self.ble_advertise_callback)
-    self.ble_advertise_callback = None
+    def deactivate_ble_pairing_mode(self) -> None:
+        """Deactivates BLE pairing mode on an AndroidDevice."""
+        if not self.ble_advertise_callback:
+            self._ad.log.debug('BLE pairing mode is not activated.')
+            return
+        logging.info('Deactivating BLE pairing mode on AndroidDevice.')
+        self._ad.sl4a.bleStopBleAdvertising(self.ble_advertise_callback)
+        self.ble_advertise_callback = None
 
-  def get_bluetooth_mac_address(self) -> str:
-    """Gets Bluetooth mac address of an AndroidDevice."""
-    logging.info('Getting Bluetooth mac address for AndroidDevice.')
-    mac_address = self._ad.sl4a.bluetoothGetLocalAddress()
-    logging.info('Bluetooth mac address of AndroidDevice: %s', mac_address)
-    return mac_address
+    def get_bluetooth_mac_address(self) -> str:
+        """Gets Bluetooth mac address of an AndroidDevice."""
+        logging.info('Getting Bluetooth mac address for AndroidDevice.')
+        mac_address = self._ad.sl4a.bluetoothGetLocalAddress()
+        logging.info('Bluetooth mac address of AndroidDevice: %s', mac_address)
+        return mac_address
 
-  def scan_and_get_ble_device_address(
-      self,
-      device_name: str,
-      timeout_sec: float = 30) -> str:
-    """Searchs a BLE device by BLE scanner and returns it's BLE mac address.
+    def scan_and_get_ble_device_address(self, device_name: str, timeout_sec: float = 30) -> str:
+        """Searchs a BLE device by BLE scanner and returns it's BLE mac address.
 
     Args:
       device_name: string, the name of BLE device.
@@ -618,43 +559,38 @@
     Raises:
       ControllerError: Raised if failed to get the BLE device address
     """
-    filter_list = self._ad.sl4a.bleGenFilterList()
-    scan_settings = self._ad.sl4a.bleBuildScanSetting()
-    scan_callback = self._ad.sl4a.bleGenScanCallback()
-    self._ad.sl4a.bleSetScanFilterDeviceName(device_name)
-    self._ad.sl4a.bleBuildScanFilter(filter_list)
-    self._ad.sl4a.bleStartBleScan(filter_list, scan_settings, scan_callback)
-    try:
-      event = self._ad.ed.pop_event(
-          'BleScan%sonScanResults' % scan_callback, timeout_sec)
-    except queue.Empty:
-      raise signals.ControllerError(
-          'Timed out %ds after waiting for phone finding BLE device: %s.' %
-          (timeout_sec, device_name))
-    finally:
-      self._ad.sl4a.bleStopBleScan(scan_callback)
-    return event['data']['Result']['deviceInfo']['address']
+        filter_list = self._ad.sl4a.bleGenFilterList()
+        scan_settings = self._ad.sl4a.bleBuildScanSetting()
+        scan_callback = self._ad.sl4a.bleGenScanCallback()
+        self._ad.sl4a.bleSetScanFilterDeviceName(device_name)
+        self._ad.sl4a.bleBuildScanFilter(filter_list)
+        self._ad.sl4a.bleStartBleScan(filter_list, scan_settings, scan_callback)
+        try:
+            event = self._ad.ed.pop_event('BleScan%sonScanResults' % scan_callback, timeout_sec)
+        except queue.Empty:
+            raise signals.ControllerError(
+                'Timed out %ds after waiting for phone finding BLE device: %s.' % (timeout_sec, device_name))
+        finally:
+            self._ad.sl4a.bleStopBleScan(scan_callback)
+        return event['data']['Result']['deviceInfo']['address']
 
-  def get_device_name(self) -> str:
-    """Gets Bluetooth device name of an AndroidDevice."""
-    logging.info('Getting Bluetooth device name for AndroidDevice.')
-    device_name = self._ad.sl4a.bluetoothGetLocalName()
-    logging.info('Bluetooth device name of AndroidDevice: %s', device_name)
-    return device_name
+    def get_device_name(self) -> str:
+        """Gets Bluetooth device name of an AndroidDevice."""
+        logging.info('Getting Bluetooth device name for AndroidDevice.')
+        device_name = self._ad.sl4a.bluetoothGetLocalName()
+        logging.info('Bluetooth device name of AndroidDevice: %s', device_name)
+        return device_name
 
-  def is_bluetooth_sco_on(self) -> bool:
-    """Checks whether communications use Bluetooth SCO."""
-    cmd = 'dumpsys bluetooth_manager | grep "isBluetoothScoOn"'
-    get_status = self._ad.adb.shell(cmd)
-    if isinstance(get_status, bytes):
-      get_status = get_status.decode()
-    return 'true' in get_status
+    def is_bluetooth_sco_on(self) -> bool:
+        """Checks whether communications use Bluetooth SCO."""
+        cmd = 'dumpsys bluetooth_manager | grep "isBluetoothScoOn"'
+        get_status = self._ad.adb.shell(cmd)
+        if isinstance(get_status, bytes):
+            get_status = get_status.decode()
+        return 'true' in get_status
 
-  def connect_with_profile(
-      self,
-      snd_ad_mac_address: str,
-      profile: bt_constants.BluetoothProfile) -> bool:
-    """Connects with the profile.
+    def connect_with_profile(self, snd_ad_mac_address: str, profile: bt_constants.BluetoothProfile) -> bool:
+        """Connects with the profile.
 
     The connection can only be completed after the bluetooth devices are paired.
     To connected with the profile, the bluetooth connection policy is set to
@@ -669,33 +605,29 @@
     Returns:
       The profile connection succeed/fail
     """
-    if profile == bt_constants.BluetoothProfile.MAP_MCE:
-      self._ad.sl4a.bluetoothMapClientConnect(snd_ad_mac_address)
-    elif profile == bt_constants.BluetoothProfile.PBAP_CLIENT:
-      self.set_profile_policy(
-          snd_ad_mac_address, profile,
-          bt_constants.BluetoothConnectionPolicy.CONNECTION_POLICY_ALLOWED)
-      self._ad.sl4a.bluetoothPbapClientConnect(snd_ad_mac_address)
-    else:
-      self.set_profile_policy(
-          snd_ad_mac_address, profile,
-          bt_constants.BluetoothConnectionPolicy.CONNECTION_POLICY_FORBIDDEN)
-      self.set_profile_policy(
-          snd_ad_mac_address, profile,
-          bt_constants.BluetoothConnectionPolicy.CONNECTION_POLICY_ALLOWED)
-      self._ad.sl4a.bluetoothConnectBonded(snd_ad_mac_address)
-    time.sleep(BT_CONNECTION_WAITING_TIME_SECONDS)
-    is_connected = self._is_profile_connected(snd_ad_mac_address, profile)
-    self.log.info('The connection between %s and %s for profile %s succeed: %s',
-                  self.serial, snd_ad_mac_address, profile, is_connected)
-    return is_connected
+        if profile == bt_constants.BluetoothProfile.MAP_MCE:
+            self._ad.sl4a.bluetoothMapClientConnect(snd_ad_mac_address)
+        elif profile == bt_constants.BluetoothProfile.PBAP_CLIENT:
+            self.set_profile_policy(snd_ad_mac_address, profile,
+                                    bt_constants.BluetoothConnectionPolicy.CONNECTION_POLICY_ALLOWED)
+            self._ad.sl4a.bluetoothPbapClientConnect(snd_ad_mac_address)
+        else:
+            self.set_profile_policy(snd_ad_mac_address, profile,
+                                    bt_constants.BluetoothConnectionPolicy.CONNECTION_POLICY_FORBIDDEN)
+            self.set_profile_policy(snd_ad_mac_address, profile,
+                                    bt_constants.BluetoothConnectionPolicy.CONNECTION_POLICY_ALLOWED)
+            self._ad.sl4a.bluetoothConnectBonded(snd_ad_mac_address)
+        time.sleep(BT_CONNECTION_WAITING_TIME_SECONDS)
+        is_connected = self._is_profile_connected(snd_ad_mac_address, profile)
+        self.log.info('The connection between %s and %s for profile %s succeed: %s', self.serial, snd_ad_mac_address,
+                      profile, is_connected)
+        return is_connected
 
-  def connect_to_snd_with_profile(
-      self,
-      snd_ad: android_device.AndroidDevice,
-      profile: bt_constants.BluetoothProfile,
-      attempts: int = 5) -> bool:
-    """Connects pri android device to snd android device with profile.
+    def connect_to_snd_with_profile(self,
+                                    snd_ad: android_device.AndroidDevice,
+                                    profile: bt_constants.BluetoothProfile,
+                                    attempts: int = 5) -> bool:
+        """Connects pri android device to snd android device with profile.
 
     Args:
         snd_ad: android device accepting connection
@@ -705,27 +637,24 @@
     Returns:
         Boolean of connecting result
     """
-    pri_ad = self._ad
-    curr_attempts = 0
-    snd_ad_mac_address = snd_ad.sl4a.bluetoothGetLocalAddress()
-    if not self.is_bt_paired(snd_ad_mac_address):
-      self.log.error('Devices %s and %s not paired before connecting',
-                     self.serial, snd_ad.serial)
-      return False
-    while curr_attempts < attempts:
-      curr_attempts += 1
-      self.log.info('Connection of profile %s at curr attempt %d (total %d)',
-                    profile, curr_attempts, attempts)
-      if self.connect_with_profile(snd_ad_mac_address, profile):
-        self.log.info('Connection between devices %s and %s succeeds at %d try',
-                      pri_ad.serial, snd_ad.serial, curr_attempts)
-        return True
-    self.log.error('Connection of profile %s failed after %d attempts', profile,
-                   attempts)
-    return False
+        pri_ad = self._ad
+        curr_attempts = 0
+        snd_ad_mac_address = snd_ad.sl4a.bluetoothGetLocalAddress()
+        if not self.is_bt_paired(snd_ad_mac_address):
+            self.log.error('Devices %s and %s not paired before connecting', self.serial, snd_ad.serial)
+            return False
+        while curr_attempts < attempts:
+            curr_attempts += 1
+            self.log.info('Connection of profile %s at curr attempt %d (total %d)', profile, curr_attempts, attempts)
+            if self.connect_with_profile(snd_ad_mac_address, profile):
+                self.log.info('Connection between devices %s and %s succeeds at %d try', pri_ad.serial, snd_ad.serial,
+                              curr_attempts)
+                return True
+        self.log.error('Connection of profile %s failed after %d attempts', profile, attempts)
+        return False
 
-  def is_bt_paired(self, mac_address: str) -> bool:
-    """Check if the bluetooth device with mac_address is paired to ad.
+    def is_bt_paired(self, mac_address: str) -> bool:
+        """Check if the bluetooth device with mac_address is paired to ad.
 
     Args:
       mac_address: the mac address of the bluetooth device for pairing
@@ -733,11 +662,11 @@
     Returns:
       True if they are paired
     """
-    bonded_info = self._ad.sl4a.bluetoothGetBondedDevices()
-    return mac_address in [info['address'] for info in bonded_info]
+        bonded_info = self._ad.sl4a.bluetoothGetBondedDevices()
+        return mac_address in [info['address'] for info in bonded_info]
 
-  def is_a2dp_sink_connected(self, mac_address: str) -> bool:
-    """Checks if the Android device connects to a A2DP sink device.
+    def is_a2dp_sink_connected(self, mac_address: str) -> bool:
+        """Checks if the Android device connects to a A2DP sink device.
 
     Args:
       mac_address: String, Bluetooth MAC address of the A2DP sink device.
@@ -745,11 +674,11 @@
     Returns:
       True if connected else False.
     """
-    connected_devices = self._ad.sl4a.bluetoothA2dpGetConnectedDevices()
-    return mac_address in [d['address'] for d in connected_devices]
+        connected_devices = self._ad.sl4a.bluetoothA2dpGetConnectedDevices()
+        return mac_address in [d['address'] for d in connected_devices]
 
-  def hfp_connect(self, ag_ad: android_device.AndroidDevice) -> bool:
-    """Hfp connecting hf android device to ag android device.
+    def hfp_connect(self, ag_ad: android_device.AndroidDevice) -> bool:
+        """Hfp connecting hf android device to ag android device.
 
     The android device should support the Headset Client profile. For example,
     the android device with git_master-bds-dev build.
@@ -760,11 +689,10 @@
     Returns:
         Boolean of connecting result
     """
-    return self.connect_to_snd_with_profile(
-        ag_ad, bt_constants.BluetoothProfile.HEADSET_CLIENT)
+        return self.connect_to_snd_with_profile(ag_ad, bt_constants.BluetoothProfile.HEADSET_CLIENT)
 
-  def a2dp_sink_connect(self, src_ad: android_device.AndroidDevice) -> bool:
-    """Connects pri android device to secondary android device.
+    def a2dp_sink_connect(self, src_ad: android_device.AndroidDevice) -> bool:
+        """Connects pri android device to secondary android device.
 
     The android device should support the A2dp Sink profile. For example, the
     android device with git_master-bds-dev build.
@@ -775,11 +703,10 @@
     Returns:
       Boolean of connecting result
     """
-    return self.connect_to_snd_with_profile(
-        src_ad, bt_constants.BluetoothProfile.A2DP_SINK)
+        return self.connect_to_snd_with_profile(src_ad, bt_constants.BluetoothProfile.A2DP_SINK)
 
-  def map_connect(self, map_ad: android_device.AndroidDevice) -> bool:
-    """Connects primary device to secondary device via MAP MCE profile.
+    def map_connect(self, map_ad: android_device.AndroidDevice) -> bool:
+        """Connects primary device to secondary device via MAP MCE profile.
 
     The primary device should support the MAP MCE profile. For example,
     the android device with git_master-bds-dev build.
@@ -790,11 +717,10 @@
     Returns:
         Boolean of connecting result
     """
-    return self.connect_to_snd_with_profile(
-        map_ad, bt_constants.BluetoothProfile.MAP_MCE)
+        return self.connect_to_snd_with_profile(map_ad, bt_constants.BluetoothProfile.MAP_MCE)
 
-  def map_disconnect(self, bluetooth_address: str) -> bool:
-    """Disconnects a MAP MSE device with specified Bluetooth MAC address.
+    def map_disconnect(self, bluetooth_address: str) -> bool:
+        """Disconnects a MAP MSE device with specified Bluetooth MAC address.
 
     Args:
       bluetooth_address: a connected device's bluetooth address.
@@ -802,15 +728,15 @@
     Returns:
       True if the device is disconnected else False.
     """
-    self._ad.sl4a.bluetoothMapClientDisconnect(bluetooth_address)
-    return bt_test_utils.wait_until(
-        timeout_sec=COMMON_TIMEOUT_SECONDS,
-        condition_func=self._is_profile_connected,
-        func_args=[bluetooth_address, bt_constants.BluetoothProfile.MAP_MCE],
-        expected_value=False)
+        self._ad.sl4a.bluetoothMapClientDisconnect(bluetooth_address)
+        return bt_test_utils.wait_until(
+            timeout_sec=COMMON_TIMEOUT_SECONDS,
+            condition_func=self._is_profile_connected,
+            func_args=[bluetooth_address, bt_constants.BluetoothProfile.MAP_MCE],
+            expected_value=False)
 
-  def pbap_connect(self, pbap_ad: android_device.AndroidDevice) -> bool:
-    """Connects primary device to secondary device via PBAP client profile.
+    def pbap_connect(self, pbap_ad: android_device.AndroidDevice) -> bool:
+        """Connects primary device to secondary device via PBAP client profile.
 
     The primary device should support the PBAP client profile. For example,
     the android device with git_master-bds-dev build.
@@ -821,40 +747,33 @@
     Returns:
         Boolean of connecting result
     """
-    return self.connect_to_snd_with_profile(
-        pbap_ad, bt_constants.BluetoothProfile.PBAP_CLIENT)
+        return self.connect_to_snd_with_profile(pbap_ad, bt_constants.BluetoothProfile.PBAP_CLIENT)
 
-  def set_bluetooth_tethering(self, status_enabled: bool) -> None:
-    """Sets Bluetooth tethering to be specific status.
+    def set_bluetooth_tethering(self, status_enabled: bool) -> None:
+        """Sets Bluetooth tethering to be specific status.
 
     Args:
       status_enabled: Bool, Bluetooth tethering will be set to enable if True,
           else disable.
     """
-    if self._ad.sl4a.bluetoothPanIsTetheringOn() == status_enabled:
-      self._ad.log.info('Already %s Bluetooth tethering.' %
-                        ('enabled' if status_enabled else 'disabled'))
-      return
+        if self._ad.sl4a.bluetoothPanIsTetheringOn() == status_enabled:
+            self._ad.log.info('Already %s Bluetooth tethering.' % ('enabled' if status_enabled else 'disabled'))
+            return
 
-    self._ad.log.info('%s Bluetooth tethering.' %
-                      ('Enable' if status_enabled else 'Disable'))
-    self._ad.sl4a.bluetoothPanSetBluetoothTethering(status_enabled)
+        self._ad.log.info('%s Bluetooth tethering.' % ('Enable' if status_enabled else 'Disable'))
+        self._ad.sl4a.bluetoothPanSetBluetoothTethering(status_enabled)
 
-    bt_test_utils.wait_until(
-        timeout_sec=COMMON_TIMEOUT_SECONDS,
-        condition_func=self._ad.sl4a.bluetoothPanIsTetheringOn,
-        func_args=[],
-        expected_value=status_enabled,
-        exception=signals.ControllerError(
-            'Failed to %s Bluetooth tethering.' %
-            ('enable' if status_enabled else 'disable')))
+        bt_test_utils.wait_until(
+            timeout_sec=COMMON_TIMEOUT_SECONDS,
+            condition_func=self._ad.sl4a.bluetoothPanIsTetheringOn,
+            func_args=[],
+            expected_value=status_enabled,
+            exception=signals.ControllerError('Failed to %s Bluetooth tethering.' % ('enable'
+                                                                                     if status_enabled else 'disable')))
 
-  def set_profile_policy(
-      self,
-      snd_ad_mac_address: str,
-      profile: bt_constants.BluetoothProfile,
-      policy: bt_constants.BluetoothConnectionPolicy) -> None:
-    """Sets policy of the profile car related profiles to OFF.
+    def set_profile_policy(self, snd_ad_mac_address: str, profile: bt_constants.BluetoothProfile,
+                           policy: bt_constants.BluetoothConnectionPolicy) -> None:
+        """Sets policy of the profile car related profiles to OFF.
 
     This avoids autoconnect being triggered randomly. The use of this function
     is encouraged when you're testing individual profiles in isolation.
@@ -864,47 +783,40 @@
       profile: the profiles to be set
       policy: the policy value to be set
     """
-    pri_ad = self._ad
-    pri_ad_local_name = pri_ad.sl4a.bluetoothGetLocalName()
-    pri_ad.log.info('Sets profile %s on %s for %s to policy %s', profile,
-                    pri_ad_local_name, snd_ad_mac_address, policy)
-    if profile == bt_constants.BluetoothProfile.A2DP:
-      pri_ad.sl4a.bluetoothA2dpSetPriority(snd_ad_mac_address, policy.value)
-    elif profile == bt_constants.BluetoothProfile.A2DP_SINK:
-      pri_ad.sl4a.bluetoothA2dpSinkSetPriority(snd_ad_mac_address, policy.value)
-    elif profile == bt_constants.BluetoothProfile.HEADSET_CLIENT:
-      pri_ad.sl4a.bluetoothHfpClientSetPriority(snd_ad_mac_address,
-                                                policy.value)
-    elif profile == bt_constants.BluetoothProfile.PBAP_CLIENT:
-      pri_ad.sl4a.bluetoothPbapClientSetPriority(snd_ad_mac_address,
-                                                 policy.value)
-    elif profile == bt_constants.BluetoothProfile.HID_HOST:
-      pri_ad.sl4a.bluetoothHidSetPriority(snd_ad_mac_address, policy.value)
-    else:
-      pri_ad.log.error('Profile %s not yet supported for policy settings',
-                       profile)
+        pri_ad = self._ad
+        pri_ad_local_name = pri_ad.sl4a.bluetoothGetLocalName()
+        pri_ad.log.info('Sets profile %s on %s for %s to policy %s', profile, pri_ad_local_name, snd_ad_mac_address,
+                        policy)
+        if profile == bt_constants.BluetoothProfile.A2DP:
+            pri_ad.sl4a.bluetoothA2dpSetPriority(snd_ad_mac_address, policy.value)
+        elif profile == bt_constants.BluetoothProfile.A2DP_SINK:
+            pri_ad.sl4a.bluetoothA2dpSinkSetPriority(snd_ad_mac_address, policy.value)
+        elif profile == bt_constants.BluetoothProfile.HEADSET_CLIENT:
+            pri_ad.sl4a.bluetoothHfpClientSetPriority(snd_ad_mac_address, policy.value)
+        elif profile == bt_constants.BluetoothProfile.PBAP_CLIENT:
+            pri_ad.sl4a.bluetoothPbapClientSetPriority(snd_ad_mac_address, policy.value)
+        elif profile == bt_constants.BluetoothProfile.HID_HOST:
+            pri_ad.sl4a.bluetoothHidSetPriority(snd_ad_mac_address, policy.value)
+        else:
+            pri_ad.log.error('Profile %s not yet supported for policy settings', profile)
 
-  def set_profiles_policy(
-      self,
-      snd_ad: android_device.AndroidDevice,
-      profile_list: Sequence[bt_constants.BluetoothProfile],
-      policy: bt_constants.BluetoothConnectionPolicy) -> None:
-    """Sets the policy of said profile(s) on pri_ad for snd_ad.
+    def set_profiles_policy(self, snd_ad: android_device.AndroidDevice,
+                            profile_list: Sequence[bt_constants.BluetoothProfile],
+                            policy: bt_constants.BluetoothConnectionPolicy) -> None:
+        """Sets the policy of said profile(s) on pri_ad for snd_ad.
 
     Args:
       snd_ad: android device accepting connection
       profile_list: list of the profiles to be set
       policy: the policy to be set
     """
-    mac_address = snd_ad.sl4a.bluetoothGetLocalAddress()
-    for profile in profile_list:
-      self.set_profile_policy(mac_address, profile, policy)
+        mac_address = snd_ad.sl4a.bluetoothGetLocalAddress()
+        for profile in profile_list:
+            self.set_profile_policy(mac_address, profile, policy)
 
-  def set_profiles_policy_off(
-      self,
-      snd_ad: android_device.AndroidDevice,
-      profile_list: Sequence[bt_constants.BluetoothProfile]) -> None:
-    """Sets policy of the profiles to OFF.
+    def set_profiles_policy_off(self, snd_ad: android_device.AndroidDevice,
+                                profile_list: Sequence[bt_constants.BluetoothProfile]) -> None:
+        """Sets policy of the profiles to OFF.
 
     This avoids autoconnect being triggered randomly. The use of this function
     is encouraged when you're testing individual profiles in isolation
@@ -913,16 +825,14 @@
       snd_ad: android device accepting connection
       profile_list: list of the profiles to be turned off
     """
-    self.set_profiles_policy(
-        snd_ad, profile_list,
-        bt_constants.BluetoothConnectionPolicy.CONNECTION_POLICY_FORBIDDEN)
+        self.set_profiles_policy(snd_ad, profile_list,
+                                 bt_constants.BluetoothConnectionPolicy.CONNECTION_POLICY_FORBIDDEN)
 
-  def wait_for_call_state(
-      self,
-      call_state: Union[int, bt_constants.CallState],
-      timeout_sec: float,
-      wait_interval: int = 3) -> bool:
-    """Waits for call state of the device to be changed.
+    def wait_for_call_state(self,
+                            call_state: Union[int, bt_constants.CallState],
+                            timeout_sec: float,
+                            wait_interval: int = 3) -> bool:
+        """Waits for call state of the device to be changed.
 
     Args:
       call_state: int, the expected call state. Call state values are:
@@ -935,28 +845,23 @@
     Returns:
       True if the call state has been changed else False.
     """
-    # TODO(user): Force external call to use CallState instead of int
-    if isinstance(call_state, bt_constants.CallState):
-      call_state = call_state.value
-    expiration_time = time.time() + timeout_sec
-    which_cycle = 1
-    while time.time() < expiration_time:
-      # Waits for the call state change in every cycle.
-      time.sleep(wait_interval)
-      self._ad.log.info(
-          'in cycle %d of waiting for call state %d', which_cycle, call_state)
-      if call_state == self._ad.mbs.getTelephonyCallState():
-        return True
-    self._ad.log.info('The call state did not change to %d before timeout',
-                      call_state)
-    return False
+        # TODO(user): Force external call to use CallState instead of int
+        if isinstance(call_state, bt_constants.CallState):
+            call_state = call_state.value
+        expiration_time = time.time() + timeout_sec
+        which_cycle = 1
+        while time.time() < expiration_time:
+            # Waits for the call state change in every cycle.
+            time.sleep(wait_interval)
+            self._ad.log.info('in cycle %d of waiting for call state %d', which_cycle, call_state)
+            if call_state == self._ad.mbs.getTelephonyCallState():
+                return True
+        self._ad.log.info('The call state did not change to %d before timeout', call_state)
+        return False
 
-  def add_call_log(
-      self,
-      call_log_type: Union[int, bt_constants.CallLogType],
-      phone_number: str,
-      call_time: int) -> None:
-    """Add call number and time to specified log.
+    def add_call_log(self, call_log_type: Union[int, bt_constants.CallLogType], phone_number: str,
+                     call_time: int) -> None:
+        """Add call number and time to specified log.
 
     Args:
       call_log_type: int, number of call log type. Call log type values are:
@@ -969,17 +874,17 @@
     Returns:
       None
     """
-    # TODO(user): Force external call to use CallLogType instead of int
-    if isinstance(call_log_type, bt_constants.CallLogType):
-      call_log_type = call_log_type.value
-    new_call_log = {}
-    new_call_log['type'] = str(call_log_type)
-    new_call_log['number'] = phone_number
-    new_call_log['time'] = str(call_time)
-    self._ad.sl4a.callLogsPut(new_call_log)
+        # TODO(user): Force external call to use CallLogType instead of int
+        if isinstance(call_log_type, bt_constants.CallLogType):
+            call_log_type = call_log_type.value
+        new_call_log = {}
+        new_call_log['type'] = str(call_log_type)
+        new_call_log['number'] = phone_number
+        new_call_log['time'] = str(call_time)
+        self._ad.sl4a.callLogsPut(new_call_log)
 
-  def get_call_volume(self) -> int:
-    """Gets current call volume of an AndroidDevice when Bluetooth SCO On.
+    def get_call_volume(self) -> int:
+        """Gets current call volume of an AndroidDevice when Bluetooth SCO On.
 
     Returns:
       An integer specifying the number of current call volume level.
@@ -987,19 +892,16 @@
     Raises:
       Error: If the pattern search failed.
     """
-    cmd = 'dumpsys audio | grep "STREAM_BLUETOOTH_SCO" | tail -1'
-    out = self._ad.adb.shell(cmd).decode()
-    pattern = r'(?<=SCO index:)\d+'
-    result = re.search(pattern, out)
-    if result is None:
-      raise Error(f'Pattern "{pattern}" search failed, dump output: {out}')
-    return int(result.group())
+        cmd = 'dumpsys audio | grep "STREAM_BLUETOOTH_SCO" | tail -1'
+        out = self._ad.adb.shell(cmd).decode()
+        pattern = r'(?<=SCO index:)\d+'
+        result = re.search(pattern, out)
+        if result is None:
+            raise Error(f'Pattern "{pattern}" search failed, dump output: {out}')
+        return int(result.group())
 
-  def make_phone_call(
-      self,
-      callee: android_device.AndroidDevice,
-      timeout_sec: float = 30) -> None:
-    """Make a phone call to callee and check if callee is ringing.
+    def make_phone_call(self, callee: android_device.AndroidDevice, timeout_sec: float = 30) -> None:
+        """Make a phone call to callee and check if callee is ringing.
 
     Args:
       callee: AndroidDevice, The callee in the phone call.
@@ -1008,18 +910,13 @@
     Raises:
       TestError
     """
-    self._ad.sl4a.telecomCallNumber(callee.dimensions['phone_number'])
-    is_ringing = callee.wait_for_call_state(bt_constants.CALL_STATE_RINGING,
-                                            timeout_sec)
-    if not is_ringing:
-      raise signals.TestError(
-          'Timed out after %ds waiting for call state: RINGING' % timeout_sec)
+        self._ad.sl4a.telecomCallNumber(callee.dimensions['phone_number'])
+        is_ringing = callee.wait_for_call_state(bt_constants.CALL_STATE_RINGING, timeout_sec)
+        if not is_ringing:
+            raise signals.TestError('Timed out after %ds waiting for call state: RINGING' % timeout_sec)
 
-  def wait_for_disconnection_success(
-      self,
-      mac_address: str,
-      timeout: float = 30) -> float:
-    """Waits for a device to connect with the AndroidDevice.
+    def wait_for_disconnection_success(self, mac_address: str, timeout: float = 30) -> float:
+        """Waits for a device to connect with the AndroidDevice.
 
     Args:
       mac_address: The Bluetooth mac address of the peripheral device.
@@ -1031,21 +928,18 @@
     Raises:
       ControllerError
     """
-    start_time = time.time()
-    end_time = start_time + timeout
-    while time.time() < end_time:
-      if not self._ad.sl4a.bluetoothIsDeviceConnected(mac_address):
-        disconnection_time = (time.time() - start_time)
-        logging.info('Disconnected device %s in %d seconds', mac_address,
-                     disconnection_time)
-        return disconnection_time
+        start_time = time.time()
+        end_time = start_time + timeout
+        while time.time() < end_time:
+            if not self._ad.sl4a.bluetoothIsDeviceConnected(mac_address):
+                disconnection_time = (time.time() - start_time)
+                logging.info('Disconnected device %s in %d seconds', mac_address, disconnection_time)
+                return disconnection_time
 
-    raise signals.ControllerError(
-        'Failed to disconnect device within %d seconds.' % timeout)
+        raise signals.ControllerError('Failed to disconnect device within %d seconds.' % timeout)
 
-  def first_pair_and_connect_bluetooth(
-      self, bt_device: Any) -> None:
-    """Pairs and connects an AndroidDevice with a Bluetooth device.
+    def first_pair_and_connect_bluetooth(self, bt_device: Any) -> None:
+        """Pairs and connects an AndroidDevice with a Bluetooth device.
 
     This method does factory reset bluetooth first and then pairs and connects
     the devices.
@@ -1057,25 +951,21 @@
     Returns:
       None
     """
-    bt_device.factory_reset_bluetooth()
-    mac_address = bt_device.get_bluetooth_mac_address()
-    bt_device.activate_pairing_mode()
-    self.pair_and_connect_bluetooth(mac_address)
+        bt_device.factory_reset_bluetooth()
+        mac_address = bt_device.get_bluetooth_mac_address()
+        bt_device.activate_pairing_mode()
+        self.pair_and_connect_bluetooth(mac_address)
 
-  def get_device_time(self) -> str:
-    """Get device epoch time and transfer to logcat timestamp format.
+    def get_device_time(self) -> str:
+        """Get device epoch time and transfer to logcat timestamp format.
 
     Returns:
       String of the device time.
     """
-    return self._ad.adb.shell(
-        'date +"%m-%d %H:%M:%S.000"').decode().splitlines()[0]
+        return self._ad.adb.shell('date +"%m-%d %H:%M:%S.000"').decode().splitlines()[0]
 
-  def logcat_filter(
-      self,
-      start_time: str,
-      text_filter: str = '') -> str:
-    """Returns logcat after a given time.
+    def logcat_filter(self, start_time: str, text_filter: str = '') -> str:
+        """Returns logcat after a given time.
 
     This method calls from the android_device logcat service file and filters
     all logcat line prior to the start_time.
@@ -1090,30 +980,26 @@
     Raises:
       ValueError Exception if start_time is invalid format.
     """
-    try:
-      start_time_conv = datetime.datetime.strptime(start_time, _DATETIME_FMT)
-    except ValueError as ex:
-      logging.error('Invalid time format!')
-      raise ex
-    logcat_response = ''
-    with open(self._ad.adb_logcat_file_path, 'r', errors='replace') \
-        as logcat_file:
-      post_start_time = False
-      for line in logcat_file:
-        match = self.regex_logcat_time.match(line)
-        if match:
-          if (datetime.datetime.strptime(
-              match.group('datetime'), _DATETIME_FMT) >= start_time_conv):
-            post_start_time = True
-          if post_start_time and line.find(text_filter) >= 0:
-            logcat_response += line
-    return logcat_response
+        try:
+            start_time_conv = datetime.datetime.strptime(start_time, _DATETIME_FMT)
+        except ValueError as ex:
+            logging.error('Invalid time format!')
+            raise ex
+        logcat_response = ''
+        with open(self._ad.adb_logcat_file_path, 'r', errors='replace') \
+            as logcat_file:
+            post_start_time = False
+            for line in logcat_file:
+                match = self.regex_logcat_time.match(line)
+                if match:
+                    if (datetime.datetime.strptime(match.group('datetime'), _DATETIME_FMT) >= start_time_conv):
+                        post_start_time = True
+                    if post_start_time and line.find(text_filter) >= 0:
+                        logcat_response += line
+        return logcat_response
 
-  def logcat_filter_message(
-      self,
-      current_time: str,
-      text: str = '') -> str:
-    """DEPRECATED Builds the logcat command.
+    def logcat_filter_message(self, current_time: str, text: str = '') -> str:
+        """DEPRECATED Builds the logcat command.
 
     This method builds the logcat command to check for a specified log
     message after the specified time. If text=None, the logcat returned will be
@@ -1127,13 +1013,11 @@
     Returns:
       The response of the logcat filter.
     """
-    return self.logcat_filter(current_time, text)
+        return self.logcat_filter(current_time, text)
 
-  def send_media_passthrough_cmd(
-      self,
-      command: str,
-      event_receiver: Optional[android_device.AndroidDevice] = None) -> None:
-    """Sends a media passthrough command.
+    def send_media_passthrough_cmd(self, command: str,
+                                   event_receiver: Optional[android_device.AndroidDevice] = None) -> None:
+        """Sends a media passthrough command.
 
     Args:
       command: string, media passthrough command.
@@ -1143,87 +1027,75 @@
     Raises:
       signals.ControllerError: raised if the event is not received.
     """
-    if event_receiver is None:
-      event_receiver = self._ad
-    self._ad.log.info('Sending Media Passthough: %s' % command)
-    self._ad.sl4a.bluetoothMediaPassthrough(command)
-    if not event_receiver:
-      event_receiver = self._ad
-    try:
-      event_receiver.ed.pop_event(MEDIA_CMD_MAP[command],
-                                  MEDIA_EVENT_TIMEOUT_SEC)
-    except queue.Empty:
-      raise signals.ControllerError(
-          'Device "%s" failed to receive the event "%s" '
-          'when the command "%s" was sent.' %
-          (event_receiver.serial, MEDIA_CMD_MAP[command], command))
+        if event_receiver is None:
+            event_receiver = self._ad
+        self._ad.log.info('Sending Media Passthough: %s' % command)
+        self._ad.sl4a.bluetoothMediaPassthrough(command)
+        if not event_receiver:
+            event_receiver = self._ad
+        try:
+            event_receiver.ed.pop_event(MEDIA_CMD_MAP[command], MEDIA_EVENT_TIMEOUT_SEC)
+        except queue.Empty:
+            raise signals.ControllerError(
+                'Device "%s" failed to receive the event "%s" '
+                'when the command "%s" was sent.' % (event_receiver.serial, MEDIA_CMD_MAP[command], command))
 
-  def pause(self) -> None:
-    """Sends the AVRCP command "pause"."""
-    self.send_media_passthrough_cmd(bt_constants.CMD_MEDIA_PAUSE)
+    def pause(self) -> None:
+        """Sends the AVRCP command "pause"."""
+        self.send_media_passthrough_cmd(bt_constants.CMD_MEDIA_PAUSE)
 
-  def play(self) -> None:
-    """Sends the AVRCP command "play"."""
-    self.send_media_passthrough_cmd(bt_constants.CMD_MEDIA_PLAY)
+    def play(self) -> None:
+        """Sends the AVRCP command "play"."""
+        self.send_media_passthrough_cmd(bt_constants.CMD_MEDIA_PLAY)
 
-  def track_previous(self) -> None:
-    """Sends the AVRCP command "skipPrev"."""
-    self.send_media_passthrough_cmd(bt_constants.CMD_MEDIA_SKIP_PREV)
+    def track_previous(self) -> None:
+        """Sends the AVRCP command "skipPrev"."""
+        self.send_media_passthrough_cmd(bt_constants.CMD_MEDIA_SKIP_PREV)
 
-  def track_next(self) -> None:
-    """Sends the AVRCP command "skipNext"."""
-    self.send_media_passthrough_cmd(bt_constants.CMD_MEDIA_SKIP_NEXT)
+    def track_next(self) -> None:
+        """Sends the AVRCP command "skipNext"."""
+        self.send_media_passthrough_cmd(bt_constants.CMD_MEDIA_SKIP_NEXT)
 
-  def get_current_track_info(self) -> Dict[str, Any]:
-    """Returns Dict (Media metadata) representing the current track."""
-    return self._ad.sl4a.bluetoothMediaGetCurrentMediaMetaData()
+    def get_current_track_info(self) -> Dict[str, Any]:
+        """Returns Dict (Media metadata) representing the current track."""
+        return self._ad.sl4a.bluetoothMediaGetCurrentMediaMetaData()
 
-  def get_current_playback_state(self) -> int:
-    """Returns Integer representing the current playback state."""
-    return self._ad.sl4a.bluetoothMediaGetCurrentPlaybackState()['state']
+    def get_current_playback_state(self) -> int:
+        """Returns Integer representing the current playback state."""
+        return self._ad.sl4a.bluetoothMediaGetCurrentPlaybackState()['state']
 
-  def verify_playback_state_changed(
-      self,
-      expected_state: str,
-      exception: Optional[Exception] = None) -> bool:
-    """Verifies the playback state is changed to be the expected state.
+    def verify_playback_state_changed(self, expected_state: str, exception: Optional[Exception] = None) -> bool:
+        """Verifies the playback state is changed to be the expected state.
 
     Args:
       expected_state: string, the changed state as expected.
       exception: Exception, raised when the state is not changed if needed.
     """
-    bt_test_utils.wait_until(
-        timeout_sec=MEDIA_UPDATE_TIMEOUT_SEC,
-        condition_func=self.get_current_playback_state,
-        func_args=[],
-        expected_value=expected_state,
-        exception=exception,
-        interval_sec=1)
+        bt_test_utils.wait_until(
+            timeout_sec=MEDIA_UPDATE_TIMEOUT_SEC,
+            condition_func=self.get_current_playback_state,
+            func_args=[],
+            expected_value=expected_state,
+            exception=exception,
+            interval_sec=1)
 
-  def verify_current_track_changed(
-      self,
-      expected_track: str,
-      exception: Optional[Exception] = None) -> bool:
-    """Verifies the Now playing track is changed to be the expected track.
+    def verify_current_track_changed(self, expected_track: str, exception: Optional[Exception] = None) -> bool:
+        """Verifies the Now playing track is changed to be the expected track.
 
     Args:
       expected_track: string, the changed track as expected.
       exception: Exception, raised when the track is not changed if needed.
     """
-    bt_test_utils.wait_until(
-        timeout_sec=MEDIA_UPDATE_TIMEOUT_SEC,
-        condition_func=self.get_current_track_info,
-        func_args=[],
-        expected_value=expected_track,
-        exception=exception,
-        interval_sec=1)
+        bt_test_utils.wait_until(
+            timeout_sec=MEDIA_UPDATE_TIMEOUT_SEC,
+            condition_func=self.get_current_track_info,
+            func_args=[],
+            expected_value=expected_track,
+            exception=exception,
+            interval_sec=1)
 
-  def verify_avrcp_event(
-      self,
-      event_name: bt_constants.AvrcpEvent,
-      check_time: str,
-      timeout_sec: float = 20) -> bool:
-    """Verifies that an AVRCP event was received by an AndroidDevice.
+    def verify_avrcp_event(self, event_name: bt_constants.AvrcpEvent, check_time: str, timeout_sec: float = 20) -> bool:
+        """Verifies that an AVRCP event was received by an AndroidDevice.
 
     Checks logcat to verify that an AVRCP event was received after a given
     time.
@@ -1244,25 +1116,25 @@
     Returns:
       True if the event was received.
     """
-    avrcp_events = [
-        'State:NOT_PLAYING->PLAYING', 'State:PLAYING->NOT_PLAYING',
-        'sendMediaKeyEvent: keyEvent=76', 'sendMediaKeyEvent: keyEvent=75'
-    ]
-    if event_name.value not in avrcp_events:
-      raise signals.TestError('An unexpected AVRCP event is specified.')
+        avrcp_events = [
+            'State:NOT_PLAYING->PLAYING', 'State:PLAYING->NOT_PLAYING', 'sendMediaKeyEvent: keyEvent=76',
+            'sendMediaKeyEvent: keyEvent=75'
+        ]
+        if event_name.value not in avrcp_events:
+            raise signals.TestError('An unexpected AVRCP event is specified.')
 
-    end_time = time.time() + timeout_sec
-    while time.time() < end_time:
-      if self.logcat_filter_message(check_time, event_name.value):
-        logging.info('%s event received successfully.', event_name)
-        return True
-      time.sleep(1)
-    logging.error('AndroidDevice failed to receive %s event.', event_name)
-    logging.info('Logcat:\n%s', self.logcat_filter_message(check_time))
-    return False
+        end_time = time.time() + timeout_sec
+        while time.time() < end_time:
+            if self.logcat_filter_message(check_time, event_name.value):
+                logging.info('%s event received successfully.', event_name)
+                return True
+            time.sleep(1)
+        logging.error('AndroidDevice failed to receive %s event.', event_name)
+        logging.info('Logcat:\n%s', self.logcat_filter_message(check_time))
+        return False
 
-  def add_google_account(self, retries: int = 5) -> bool:
-    """Login Google account.
+    def add_google_account(self, retries: int = 5) -> bool:
+        """Login Google account.
 
     Args:
         retries: int, the number of retries.
@@ -1273,21 +1145,20 @@
     Raises:
       TestError
     """
-    for _ in range(retries):
-      output = self._ad.adb.shell(
-          'am instrument -w -e account "%s" -e password '
-          '"%s" -e sync true -e wait-for-checkin false '
-          'com.google.android.tradefed.account/.AddAccount' %
-          (self._ad.dimensions['google_account'],
-           self._ad.dimensions['google_account_password'])).decode()
-      if 'result=SUCCESS' in output:
-        logging.info('Google account is added successfully')
-        time.sleep(3)  # Wait for account to steady state
-        return True
-    raise signals.TestError('Failed to add google account: %s' % output)
+        for _ in range(retries):
+            output = self._ad.adb.shell(
+                'am instrument -w -e account "%s" -e password '
+                '"%s" -e sync true -e wait-for-checkin false '
+                'com.google.android.tradefed.account/.AddAccount' %
+                (self._ad.dimensions['google_account'], self._ad.dimensions['google_account_password'])).decode()
+            if 'result=SUCCESS' in output:
+                logging.info('Google account is added successfully')
+                time.sleep(3)  # Wait for account to steady state
+                return True
+        raise signals.TestError('Failed to add google account: %s' % output)
 
-  def remove_google_account(self, retries: int = 5) -> bool:
-    """Remove Google account.
+    def remove_google_account(self, retries: int = 5) -> bool:
+        """Remove Google account.
 
     Args:
         retries: int, the number of retries.
@@ -1298,18 +1169,16 @@
     Raises:
       TestError
     """
-    for _ in range(retries):
-      output = self._ad.adb.shell(
-          'am instrument -w com.google.android.tradefed.account/.RemoveAccounts'
-      ).decode()
-      if 'result=SUCCESS' in output:
-        logging.info('Google account is removed successfully')
-        return True
-      time.sleep(1)  # Buffer between retries.
-    raise signals.TestError('Failed to remove google account: %s' % output)
+        for _ in range(retries):
+            output = self._ad.adb.shell('am instrument -w com.google.android.tradefed.account/.RemoveAccounts').decode()
+            if 'result=SUCCESS' in output:
+                logging.info('Google account is removed successfully')
+                return True
+            time.sleep(1)  # Buffer between retries.
+        raise signals.TestError('Failed to remove google account: %s' % output)
 
-  def detect_and_pull_ssrdump(self, ramdump_type: str = 'ramdump_bt') -> bool:
-    """Detect and pull RAMDUMP log.
+    def detect_and_pull_ssrdump(self, ramdump_type: str = 'ramdump_bt') -> bool:
+        """Detect and pull RAMDUMP log.
 
     Args:
       ramdump_type: str, the partial of file names to search for in ramdump
@@ -1319,47 +1188,45 @@
     Returns:
       True if there is a file with file name matching the ramdump type.
     """
-    files = self._ad.adb.shell('ls %s' % bt_constants.RAMDUMP_PATH).decode()
-    if ramdump_type in files:
-      logging.info('RAMDUMP is found.')
-      log_name_timestamp = mobly_logger.get_log_file_timestamp()
-      destination = os.path.join(self._ad.log_path, 'RamdumpLogs',
-                                 log_name_timestamp)
-      utils.create_dir(destination)
-      self._ad.adb.pull([bt_constants.RAMDUMP_PATH, destination])
-      return True
-    return False
+        files = self._ad.adb.shell('ls %s' % bt_constants.RAMDUMP_PATH).decode()
+        if ramdump_type in files:
+            logging.info('RAMDUMP is found.')
+            log_name_timestamp = mobly_logger.get_log_file_timestamp()
+            destination = os.path.join(self._ad.log_path, 'RamdumpLogs', log_name_timestamp)
+            utils.create_dir(destination)
+            self._ad.adb.pull([bt_constants.RAMDUMP_PATH, destination])
+            return True
+        return False
 
-  def get_bt_num_of_crashes(self) -> int:
-    """Get number of Bluetooth crash times from bluetooth_manager.
+    def get_bt_num_of_crashes(self) -> int:
+        """Get number of Bluetooth crash times from bluetooth_manager.
 
     Returns:
       Number of Bluetooth crashed times.
     """
-    out = self._regex_bt_crash.search(
-        self._ad.adb.shell('dumpsys bluetooth_manager').decode())
-    # TODO(user): Need to consider the case "out=None" when miss in
-    # matching
-    return int(out.group('num_bt_crashes'))
+        out = self._regex_bt_crash.search(self._ad.adb.shell('dumpsys bluetooth_manager').decode())
+        # TODO(user): Need to consider the case "out=None" when miss in
+        # matching
+        return int(out.group('num_bt_crashes'))
 
-  def clean_ssrdump(self) -> None:
-    """Clean RAMDUMP log.
+    def clean_ssrdump(self) -> None:
+        """Clean RAMDUMP log.
 
     Returns:
       None
     """
-    self._ad.adb.shell('rm -rf %s/*' % bt_constants.RAMDUMP_PATH)
+        self._ad.adb.shell('rm -rf %s/*' % bt_constants.RAMDUMP_PATH)
 
-  def set_target(self, bt_device: derived_bt_device.BtDevice) -> None:
-    """Allows for use to get target device object for target interaction."""
-    self._target_device = bt_device
+    def set_target(self, bt_device: derived_bt_device.BtDevice) -> None:
+        """Allows for use to get target device object for target interaction."""
+        self._target_device = bt_device
 
-  def wait_for_hsp_connection_state(self,
-                                    mac_address: str,
-                                    connected: bool,
-                                    raise_error: bool = True,
-                                    timeout_sec: float = 30) -> bool:
-    """Waits for HSP connection to be in a expected state on Android device.
+    def wait_for_hsp_connection_state(self,
+                                      mac_address: str,
+                                      connected: bool,
+                                      raise_error: bool = True,
+                                      timeout_sec: float = 30) -> bool:
+        """Waits for HSP connection to be in a expected state on Android device.
 
     Args:
       mac_address: The Bluetooth mac address of the peripheral device.
@@ -1370,44 +1237,40 @@
     Returns:
       True if HSP connection state is the expected state.
     """
-    expected_state = bt_constants.BluetoothConnectionStatus.STATE_DISCONNECTED
-    if connected:
-      expected_state = bt_constants.BluetoothConnectionStatus.STATE_CONNECTED
-    msg = ('Failed to %s the device "%s" within %d seconds via HSP.' %
-           ('connect' if connected else 'disconnect', mac_address, timeout_sec))
-    return bt_test_utils.wait_until(
-        timeout_sec=timeout_sec,
-        condition_func=self._ad.sl4a.bluetoothHspGetConnectionStatus,
-        func_args=[mac_address],
-        expected_value=expected_state,
-        exception=signals.TestError(msg) if raise_error else None)
+        expected_state = bt_constants.BluetoothConnectionStatus.STATE_DISCONNECTED
+        if connected:
+            expected_state = bt_constants.BluetoothConnectionStatus.STATE_CONNECTED
+        msg = ('Failed to %s the device "%s" within %d seconds via HSP.' % ('connect' if connected else 'disconnect',
+                                                                            mac_address, timeout_sec))
+        return bt_test_utils.wait_until(
+            timeout_sec=timeout_sec,
+            condition_func=self._ad.sl4a.bluetoothHspGetConnectionStatus,
+            func_args=[mac_address],
+            expected_value=expected_state,
+            exception=signals.TestError(msg) if raise_error else None)
 
-  def wait_for_bluetooth_toggle_state(self,
-                                      enabled: bool = True,
-                                      timeout_sec: float = 30) -> bool:
-    """Waits for Bluetooth to be in an expected state.
+    def wait_for_bluetooth_toggle_state(self, enabled: bool = True, timeout_sec: float = 30) -> bool:
+        """Waits for Bluetooth to be in an expected state.
 
     Args:
       enabled: True if Bluetooth status is enabled as expected.
       timeout_sec: Number of seconds to wait for Bluetooth to be in the expected
           state.
     """
-    bt_test_utils.wait_until(
-        timeout_sec=timeout_sec,
-        condition_func=self._ad.mbs.btIsEnabled,
-        func_args=[],
-        expected_value=enabled,
-        exception=signals.TestError(
-            'Bluetooth is not %s within %d seconds on the device "%s".' %
-            ('enabled' if enabled else 'disabled', timeout_sec,
-             self._ad.serial)))
+        bt_test_utils.wait_until(
+            timeout_sec=timeout_sec,
+            condition_func=self._ad.mbs.btIsEnabled,
+            func_args=[],
+            expected_value=enabled,
+            exception=signals.TestError('Bluetooth is not %s within %d seconds on the device "%s".' %
+                                        ('enabled' if enabled else 'disabled', timeout_sec, self._ad.serial)))
 
-  def wait_for_a2dp_connection_state(self,
-                                     mac_address: str,
-                                     connected: bool,
-                                     raise_error: bool = True,
-                                     timeout_sec: float = 30) -> bool:
-    """Waits for A2DP connection to be in a expected state on Android device.
+    def wait_for_a2dp_connection_state(self,
+                                       mac_address: str,
+                                       connected: bool,
+                                       raise_error: bool = True,
+                                       timeout_sec: float = 30) -> bool:
+        """Waits for A2DP connection to be in a expected state on Android device.
 
     Args:
       mac_address: The Bluetooth mac address of the peripheral device.
@@ -1418,21 +1281,18 @@
     Returns:
       True if A2DP connection state is in the expected state.
     """
-    msg = ('Failed to %s the device "%s" within %d seconds via A2DP.' %
-           ('connect' if connected else 'disconnect', mac_address, timeout_sec))
-    return bt_test_utils.wait_until(
-        timeout_sec=timeout_sec,
-        condition_func=self.is_a2dp_sink_connected,
-        func_args=[mac_address],
-        expected_value=connected,
-        exception=signals.TestError(msg) if raise_error else None)
+        msg = ('Failed to %s the device "%s" within %d seconds via A2DP.' % ('connect' if connected else 'disconnect',
+                                                                             mac_address, timeout_sec))
+        return bt_test_utils.wait_until(
+            timeout_sec=timeout_sec,
+            condition_func=self.is_a2dp_sink_connected,
+            func_args=[mac_address],
+            expected_value=connected,
+            exception=signals.TestError(msg) if raise_error else None)
 
-  def wait_for_nap_service_connection(
-      self,
-      connected_mac_addr: str,
-      state_connected: bool,
-      exception: Exception) -> bool:
-    """Waits for NAP service connection to be expected state.
+    def wait_for_nap_service_connection(self, connected_mac_addr: str, state_connected: bool,
+                                        exception: Exception) -> bool:
+        """Waits for NAP service connection to be expected state.
 
     Args:
       connected_mac_addr: String, Bluetooth Mac address is needed to be checked.
@@ -1444,26 +1304,27 @@
     Raises:
       exception: Raised if NAP service connection is not expected state.
     """
-    def is_device_connected():
-      """Returns True if connected else False."""
-      connected_devices = self._ad.sl4a.bluetoothPanGetConnectedDevices()
-      # Check if the Bluetooth mac address is in the connected device list.
-      return connected_mac_addr in [d['address'] for d in connected_devices]
 
-    bt_test_utils.wait_until(
-        timeout_sec=bt_constants.NAP_CONNECTION_TIMEOUT_SECS,
-        condition_func=is_device_connected,
-        func_args=[],
-        expected_value=state_connected,
-        exception=exception)
+        def is_device_connected():
+            """Returns True if connected else False."""
+            connected_devices = self._ad.sl4a.bluetoothPanGetConnectedDevices()
+            # Check if the Bluetooth mac address is in the connected device list.
+            return connected_mac_addr in [d['address'] for d in connected_devices]
 
-  def verify_internet(self,
-                      allow_access: bool,
-                      exception: Exception,
-                      test_url: str = TEST_URL,
-                      interval_sec: int = PING_INTERVAL_TIME_SEC,
-                      timeout_sec: float = PING_TIMEOUT_SEC) -> bool:
-    """Verifies that internet is in expected state.
+        bt_test_utils.wait_until(
+            timeout_sec=bt_constants.NAP_CONNECTION_TIMEOUT_SECS,
+            condition_func=is_device_connected,
+            func_args=[],
+            expected_value=state_connected,
+            exception=exception)
+
+    def verify_internet(self,
+                        allow_access: bool,
+                        exception: Exception,
+                        test_url: str = TEST_URL,
+                        interval_sec: int = PING_INTERVAL_TIME_SEC,
+                        timeout_sec: float = PING_TIMEOUT_SEC) -> bool:
+        """Verifies that internet is in expected state.
 
     Continuously make ping request to a URL for internet verification.
 
@@ -1480,38 +1341,34 @@
     Raises:
       exception: Raised if internet is not in expected state.
     """
-    self._ad.log.info('Verify that internet %s be used.' %
-                      ('can' if allow_access else 'can not'))
+        self._ad.log.info('Verify that internet %s be used.' % ('can' if allow_access else 'can not'))
 
-    def http_ping():
-      """Returns True if http ping success else False."""
-      try:
-        return bool(self._ad.sl4a.httpPing(test_url))
-      except jsonrpc_client_base.ApiError as e:
-        # ApiError is raised by httpPing() when no internet.
-        self._ad.log.debug(str(e))
-      return False
+        def http_ping():
+            """Returns True if http ping success else False."""
+            try:
+                return bool(self._ad.sl4a.httpPing(test_url))
+            except jsonrpc_client_base.ApiError as e:
+                # ApiError is raised by httpPing() when no internet.
+                self._ad.log.debug(str(e))
+            return False
 
-    bt_test_utils.wait_until(
-        timeout_sec=timeout_sec,
-        condition_func=http_ping,
-        func_args=[],
-        expected_value=allow_access,
-        exception=exception,
-        interval_sec=interval_sec)
+        bt_test_utils.wait_until(
+            timeout_sec=timeout_sec,
+            condition_func=http_ping,
+            func_args=[],
+            expected_value=allow_access,
+            exception=exception,
+            interval_sec=interval_sec)
 
-  def allow_extra_permissions(self) -> None:
-    """A method to allow extra permissions.
+    def allow_extra_permissions(self) -> None:
+        """A method to allow extra permissions.
 
     This method has no any logics. It is used to skip the operation when it is
     called if a test is not Wear OS use case.
     """
 
-  def is_service_running(
-      self,
-      mac_address: str,
-      timeout_sec: float) -> bool:
-    """Checks bluetooth profile state.
+    def is_service_running(self, mac_address: str, timeout_sec: float) -> bool:
+        """Checks bluetooth profile state.
 
        Check bluetooth headset/a2dp profile connection
        status from bluetooth manager log.
@@ -1524,52 +1381,41 @@
     Returns:
         True: If pattern match with bluetooth_manager_log.
     """
-    pattern_headset = (r'\sm\w+e:\sC\w+d')
-    pattern_a2dp = (r'StateMachine:.*state=Connected')
-    output_headset = self._ad.adb.shell(
-        'dumpsys bluetooth_manager | egrep -A20 "Profile: HeadsetService"'
-    ).decode()
-    output_a2dp = self._ad.adb.shell(
-        'dumpsys bluetooth_manager | egrep -A30 "Profile: A2dpService"').decode(
-        )
-    service_type = {
-        'a2dp': ((pattern_a2dp), (output_a2dp)),
-        'headset': ((pattern_headset), (output_headset))
-    }
-    start_time = time.time()
-    end_time = start_time + timeout_sec
-    while start_time < end_time:
-      try:
-        match = service_type
-        if match and mac_address in service_type:
-          return True
-      except adb.AdbError as e:
-        logging.exception(e)
-      time.sleep(ADB_WAITING_TIME_SECONDS)
-    return False
+        pattern_headset = (r'\sm\w+e:\sC\w+d')
+        pattern_a2dp = (r'StateMachine:.*state=Connected')
+        output_headset = self._ad.adb.shell('dumpsys bluetooth_manager | egrep -A20 "Profile: HeadsetService"').decode()
+        output_a2dp = self._ad.adb.shell('dumpsys bluetooth_manager | egrep -A30 "Profile: A2dpService"').decode()
+        service_type = {'a2dp': ((pattern_a2dp), (output_a2dp)), 'headset': ((pattern_headset), (output_headset))}
+        start_time = time.time()
+        end_time = start_time + timeout_sec
+        while start_time < end_time:
+            try:
+                match = service_type
+                if match and mac_address in service_type:
+                    return True
+            except adb.AdbError as e:
+                logging.exception(e)
+            time.sleep(ADB_WAITING_TIME_SECONDS)
+        return False
 
-  def connect_wifi_from_other_device_hotspot(
-      self, wifi_hotspot_device: android_device.AndroidDevice) -> None:
-    """Turns on 2.4G Wifi hotspot from the other android device and connect on the android device.
+    def connect_wifi_from_other_device_hotspot(self, wifi_hotspot_device: android_device.AndroidDevice) -> None:
+        """Turns on 2.4G Wifi hotspot from the other android device and connect on the android device.
 
     Args:
       wifi_hotspot_device: Android device, turn on 2.4G Wifi hotspot.
     """
-    wifi_hotspot_2_4g_config = bt_constants.WIFI_HOTSPOT_2_4G.copy()
-    if int(wifi_hotspot_device.build_info['build_version_sdk']) > 29:
-      wifi_hotspot_2_4g_config['apBand'] = 1
-    # Turn on 2.4G Wifi hotspot on the secondary phone.
-    wifi_hotspot_device.sl4a.wifiSetWifiApConfiguration(
-        wifi_hotspot_2_4g_config)
-    wifi_hotspot_device.sl4a.connectivityStartTethering(0, False)
-    # Connect the 2.4G Wifi on the primary phone.
-    self._ad.mbs.wifiEnable()
-    self._ad.mbs.wifiConnectSimple(
-        wifi_hotspot_2_4g_config['SSID'],
-        wifi_hotspot_2_4g_config['password'])
+        wifi_hotspot_2_4g_config = bt_constants.WIFI_HOTSPOT_2_4G.copy()
+        if int(wifi_hotspot_device.build_info['build_version_sdk']) > 29:
+            wifi_hotspot_2_4g_config['apBand'] = 1
+        # Turn on 2.4G Wifi hotspot on the secondary phone.
+        wifi_hotspot_device.sl4a.wifiSetWifiApConfiguration(wifi_hotspot_2_4g_config)
+        wifi_hotspot_device.sl4a.connectivityStartTethering(0, False)
+        # Connect the 2.4G Wifi on the primary phone.
+        self._ad.mbs.wifiEnable()
+        self._ad.mbs.wifiConnectSimple(wifi_hotspot_2_4g_config['SSID'], wifi_hotspot_2_4g_config['password'])
 
-  def get_paired_device_supported_codecs(self, mac_address: str) -> List[str]:
-    """Gets the supported A2DP codecs of the paired Bluetooth device.
+    def get_paired_device_supported_codecs(self, mac_address: str) -> List[str]:
+        """Gets the supported A2DP codecs of the paired Bluetooth device.
 
     Gets the supported A2DP codecs of the paired Bluetooth device from bluetooth
     manager log.
@@ -1580,18 +1426,17 @@
     Returns:
         A list of the A2DP codecs that the paired Bluetooth device supports.
     """
-    if not self.is_bt_paired(mac_address):
-      raise signals.TestError(
-          f'Devices {self.serial} and {mac_address} are not paired.')
-    cmd = (f'dumpsys bluetooth_manager | '
-           f'egrep -A12 "A2dpStateMachine for {mac_address}" | '
-           f'egrep -A5 "mCodecsSelectableCapabilities"')
-    paired_device_selectable_codecs = self._ad.adb.shell(cmd).decode()
-    pattern = 'codecName:(.*),mCodecType'
-    return re.findall(pattern, paired_device_selectable_codecs)
+        if not self.is_bt_paired(mac_address):
+            raise signals.TestError(f'Devices {self.serial} and {mac_address} are not paired.')
+        cmd = (f'dumpsys bluetooth_manager | '
+               f'egrep -A12 "A2dpStateMachine for {mac_address}" | '
+               f'egrep -A5 "mCodecsSelectableCapabilities"')
+        paired_device_selectable_codecs = self._ad.adb.shell(cmd).decode()
+        pattern = 'codecName:(.*),mCodecType'
+        return re.findall(pattern, paired_device_selectable_codecs)
 
-  def get_current_a2dp_codec(self) -> bt_constants.BluetoothA2dpCodec:
-    """Gets current A2DP codec type.
+    def get_current_a2dp_codec(self) -> bt_constants.BluetoothA2dpCodec:
+        """Gets current A2DP codec type.
 
     Returns:
         A number representing the current A2DP codec type.
@@ -1602,20 +1447,19 @@
         3: aptX HD
         4: LDAC
     """
-    codec_type = self._ad.sl4a.bluetoothA2dpGetCurrentCodecConfig()['codecType']
-    return bt_constants.BluetoothA2dpCodec(codec_type)
+        codec_type = self._ad.sl4a.bluetoothA2dpGetCurrentCodecConfig()['codecType']
+        return bt_constants.BluetoothA2dpCodec(codec_type)
 
-  def is_variable_bit_rate_enabled(self) -> bool:
-    """Checks if Variable Bit Rate (VBR) support is enabled for A2DP AAC codec.
+    def is_variable_bit_rate_enabled(self) -> bool:
+        """Checks if Variable Bit Rate (VBR) support is enabled for A2DP AAC codec.
 
     Returns:
       True if Variable Bit Rate support is enabled else False.
     """
-    return bt_constants.TRUE in self._ad.adb.getprop(
-        bt_constants.AAC_VBR_SUPPORTED_PROPERTY)
+        return bt_constants.TRUE in self._ad.adb.getprop(bt_constants.AAC_VBR_SUPPORTED_PROPERTY)
 
-  def toggle_variable_bit_rate(self, enabled: bool = True) -> bool:
-    """Toggles Variable Bit Rate (VBR) support status for A2DP AAC codec.
+    def toggle_variable_bit_rate(self, enabled: bool = True) -> bool:
+        """Toggles Variable Bit Rate (VBR) support status for A2DP AAC codec.
 
     After calling this method, the android device needs to restart Bluetooth for
     taking effect.
@@ -1629,14 +1473,12 @@
     Returns:
       True if the status is changed successfully else False.
     """
-    self._ad.adb.shell(
-        f'su root setprop {bt_constants.AAC_VBR_SUPPORTED_PROPERTY} '
-        f'{bt_constants.TRUE if enabled else bt_constants.FALSE}')
-    return enabled == self.is_variable_bit_rate_enabled()
+        self._ad.adb.shell(f'su root setprop {bt_constants.AAC_VBR_SUPPORTED_PROPERTY} '
+                           f'{bt_constants.TRUE if enabled else bt_constants.FALSE}')
+        return enabled == self.is_variable_bit_rate_enabled()
 
-  def pair_and_connect_ble_device(
-      self, peripheral_ble_device: android_device.AndroidDevice) -> None:
-    """Pairs Android phone with BLE device.
+    def pair_and_connect_ble_device(self, peripheral_ble_device: android_device.AndroidDevice) -> None:
+        """Pairs Android phone with BLE device.
 
     Initiates pairing from the phone and checks if it is bonded and connected to
     the BLE device.
@@ -1648,24 +1490,22 @@
     Raises:
       signals.ControllerError: raised if it failed to connect BLE device.
     """
-    peripheral_ble_device.activate_ble_pairing_mode()
-    mac_address = self.scan_and_get_ble_device_address(
-        peripheral_ble_device.get_device_name())
-    self.pair_and_connect_bluetooth(mac_address)
+        peripheral_ble_device.activate_ble_pairing_mode()
+        mac_address = self.scan_and_get_ble_device_address(peripheral_ble_device.get_device_name())
+        self.pair_and_connect_bluetooth(mac_address)
 
-  def toggle_charging(self, enabled: bool) -> None:
-    """Toggles charging on the device.
+    def toggle_charging(self, enabled: bool) -> None:
+        """Toggles charging on the device.
 
     Args:
       enabled: Enable charging if True.
     """
-    set_value = '0' if enabled else '1'
-    config_file = bt_constants.CHARGING_CONTROL_CONFIG_DICT[
-        self._ad.build_info['hardware']]
-    self._ad.adb.shell(f'echo {set_value} > {config_file}')
+        set_value = '0' if enabled else '1'
+        config_file = bt_constants.CHARGING_CONTROL_CONFIG_DICT[self._ad.build_info['hardware']]
+        self._ad.adb.shell(f'echo {set_value} > {config_file}')
 
-  def enable_airplane_mode(self, wait_secs=1) -> None:
-    """Enables airplane mode on device.
+    def enable_airplane_mode(self, wait_secs=1) -> None:
+        """Enables airplane mode on device.
 
     Args:
       wait_secs: float, the amount of time to wait after sending the airplane
@@ -1673,15 +1513,12 @@
     Returns:
         None
     """
-    self._ad.adb.shell(['settings', 'put', 'global', 'airplane_mode_on', '1'])
-    self._ad.adb.shell([
-        'am', 'broadcast', '-a', 'android.intent.action.AIRPLANE_MODE', '--ez',
-        'state', 'true'
-    ])
-    time.sleep(wait_secs)
+        self._ad.adb.shell(['settings', 'put', 'global', 'airplane_mode_on', '1'])
+        self._ad.adb.shell(['am', 'broadcast', '-a', 'android.intent.action.AIRPLANE_MODE', '--ez', 'state', 'true'])
+        time.sleep(wait_secs)
 
-  def disable_airplane_mode(self, wait_secs=1) -> None:
-    """Disables airplane mode on device.
+    def disable_airplane_mode(self, wait_secs=1) -> None:
+        """Disables airplane mode on device.
 
     Args:
       wait_secs: float, the amount of time to wait after sending the airplane
@@ -1689,22 +1526,19 @@
     Returns:
         None
     """
-    self._ad.adb.shell(['settings', 'put', 'global', 'airplane_mode_on', '0'])
-    self._ad.adb.shell([
-        'am', 'broadcast', '-a', 'android.intent.action.AIRPLANE_MODE', '--ez',
-        'state', 'false'
-    ])
-    time.sleep(wait_secs)
+        self._ad.adb.shell(['settings', 'put', 'global', 'airplane_mode_on', '0'])
+        self._ad.adb.shell(['am', 'broadcast', '-a', 'android.intent.action.AIRPLANE_MODE', '--ez', 'state', 'false'])
+        time.sleep(wait_secs)
 
-  def disable_verity_check(self) -> None:
-    """Disables Android dm verity check.
+    def disable_verity_check(self) -> None:
+        """Disables Android dm verity check.
 
     Returns:
       None
     """
-    if 'verity is already disabled' in str(self._ad.adb.disable_verity()):
-      return
-    self._ad.reboot()
-    self._ad.root_adb()
-    self._ad.wait_for_boot_completion()
-    self._ad.adb.remount()
+        if 'verity is already disabled' in str(self._ad.adb.disable_verity()):
+            return
+        self._ad.reboot()
+        self._ad.root_adb()
+        self._ad.wait_for_boot_completion()
+        self._ad.adb.remount()
diff --git a/system/blueberry/utils/ble_scan_adv_constants.py b/system/blueberry/utils/ble_scan_adv_constants.py
new file mode 100644
index 0000000..5b45c19
--- /dev/null
+++ b/system/blueberry/utils/ble_scan_adv_constants.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+#
+# Copyright (C) 2022 The Android Open Source Project
+#
+# 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.
+
+import enum
+
+
+class BleScanSettingsMatchNums(enum.IntEnum):
+    """Bluetooth Low Energy scan settings match nums"""
+    ONE = 1
+    FEW = 2
+    MAX = 3
+
+
+class BleAdvertiseSettingsTxPower(enum.IntEnum):
+    """Enum class for BLE advertise settings tx power."""
+    ULTRA_LOW = 0
+    LOW = 1
+    MEDIUM = 2
+    HIGH = 3
+
+
+class BleAdvertiseSettingsMode(enum.IntEnum):
+    """Enum class for BLE advertise settings mode."""
+    LOW_POWER = 0
+    BALANCED = 1
+    LOW_LATENCY = 2
+
+
+class BleScanSettingsModes(enum.IntEnum):
+    """Bluetooth Low Energy scan settings mode"""
+    OPPORTUNISTIC = -1
+    LOW_POWER = 0,
+    BALANCED = 1,
+    LOW_LATENCY = 2
+    AMBIENT_DISCOVERY = 3
diff --git a/system/blueberry/utils/bt_constants.py b/system/blueberry/utils/bt_constants.py
index e65e039..5772370 100644
--- a/system/blueberry/utils/bt_constants.py
+++ b/system/blueberry/utils/bt_constants.py
@@ -3,7 +3,6 @@
 
 import enum
 
-
 ### Generic Constants Begin ###
 BT_DEFAULT_TIMEOUT_SECONDS = 15
 DEFAULT_RFCOMM_TIMEOUT_MS = 10000
@@ -55,16 +54,17 @@
 
 # Dict containing charging control config for devices.
 CHARGING_CONTROL_CONFIG_DICT = {
-# Internal codename
+    # Internal codename
 }
 
 
 class AvrcpEvent(enum.Enum):
-  """Enumeration of AVRCP event types."""
-  PLAY = 'State:NOT_PLAYING->PLAYING'
-  PAUSE = 'State:PLAYING->NOT_PLAYING'
-  TRACK_PREVIOUS = 'sendMediaKeyEvent: keyEvent=76'
-  TRACK_NEXT = 'sendMediaKeyEvent: keyEvent=75'
+    """Enumeration of AVRCP event types."""
+    PLAY = 'State:NOT_PLAYING->PLAYING'
+    PAUSE = 'State:PLAYING->NOT_PLAYING'
+    TRACK_PREVIOUS = 'sendMediaKeyEvent: keyEvent=76'
+    TRACK_NEXT = 'sendMediaKeyEvent: keyEvent=75'
+
 
 # Bluetooth RFCOMM UUIDs as defined by the SIG
 BT_RFCOMM_UUIDS = {
@@ -98,115 +98,98 @@
 
 
 class BluetoothAccessLevel(enum.IntEnum):
-  """Enum class for bluetooth profile access levels."""
-  ACCESS_ALLOWED = 1
-  ACCESS_DENIED = 2
+    """Enum class for bluetooth profile access levels."""
+    ACCESS_ALLOWED = 1
+    ACCESS_DENIED = 2
 
 
 class BluetoothProfile(enum.IntEnum):
-  """Enum class for bluetooth profile types.
+    """Enum class for bluetooth profile types.
 
-  Should be kept in sync with
-  //frameworks/base/core/java/android/bluetooth/BluetoothProfile.java
-  """
-
-  HEADSET = 1
-  A2DP = 2
-  HEALTH = 3
-  HID_HOST = 4
-  PAN = 5
-  PBAP = 6
-  GATT = 7
-  GATT_SERVER = 8
-  MAP = 9
-  SAP = 10
-  A2DP_SINK = 11
-  AVRCP_CONTROLLER = 12
-  AVRCP = 13
-  HEADSET_CLIENT = 16
-  PBAP_CLIENT = 17
-  MAP_MCE = 18
-  HID_DEVICE = 19
-  OPP = 20
-  HEARING_AID = 21
+    Should be kept in sync with
+    //frameworks/base/core/java/android/bluetooth/BluetoothProfile.java
+    """
+    HEADSET = 1
+    A2DP = 2
+    HEALTH = 3
+    HID_HOST = 4
+    PAN = 5
+    PBAP = 6
+    GATT = 7
+    GATT_SERVER = 8
+    MAP = 9
+    SAP = 10
+    A2DP_SINK = 11
+    AVRCP_CONTROLLER = 12
+    AVRCP = 13
+    HEADSET_CLIENT = 16
+    PBAP_CLIENT = 17
+    MAP_MCE = 18
+    HID_DEVICE = 19
+    OPP = 20
+    HEARING_AID = 21
 
 
 class BluetoothConnectionPolicy(enum.IntEnum):
-  """Enum class for bluetooth bluetooth connection policy.
+    """Enum class for bluetooth bluetooth connection policy.
 
-  bluetooth connection policy as defined in
-  //frameworks/base/core/java/android/bluetooth/BluetoothProfile.java
-  """
-  CONNECTION_POLICY_UNKNOWN = -1
-  CONNECTION_POLICY_FORBIDDEN = 0
-  CONNECTION_POLICY_ALLOWED = 100
+    bluetooth connection policy as defined in
+    //frameworks/base/core/java/android/bluetooth/BluetoothProfile.java
+    """
+    CONNECTION_POLICY_UNKNOWN = -1
+    CONNECTION_POLICY_FORBIDDEN = 0
+    CONNECTION_POLICY_ALLOWED = 100
 
 
 class BluetoothConnectionStatus(enum.IntEnum):
-  """Enum class for bluetooth connection status.
+    """Enum class for bluetooth connection status.
 
-  Bluetooth connection status as defined in
-  //frameworks/base/core/java/android/bluetooth/BluetoothProfile.java
-  """
-  STATE_DISCONNECTED = 0
-  STATE_CONNECTING = 1
-  STATE_CONNECTED = 2
-  STATE_DISCONNECTING = 3
+    Bluetooth connection status as defined in
+    //frameworks/base/core/java/android/bluetooth/BluetoothProfile.java
+    """
+    STATE_DISCONNECTED = 0
+    STATE_CONNECTING = 1
+    STATE_CONNECTED = 2
+    STATE_DISCONNECTING = 3
 
 
 class BluetoothPriorityLevel(enum.IntEnum):
-  """Enum class for bluetooth priority level.
+    """Enum class for bluetooth priority level.
 
-  Priority levels as defined in
-  //frameworks/base/core/java/android/bluetooth/BluetoothProfile.java
-  """
-
-  PRIORITY_AUTO_CONNECT = 1000
-  PRIORITY_ON = 100
-  PRIORITY_OFF = 0
-  PRIORITY_UNDEFINED = -1
-
-
-class BleAdvertiseSettingsMode(enum.IntEnum):
-  """Enum class for BLE advertise settings mode."""
-  LOW_POWER = 0
-  BALANCED = 1
-  LOW_LATENCY = 2
-
-
-class BleAdvertiseSettingsTxPower(enum.IntEnum):
-  """Enum class for BLE advertise settings tx power."""
-  ULTRA_LOW = 0
-  LOW = 1
-  MEDIUM = 2
-  HIGH = 3
+    Priority levels as defined in
+    //frameworks/base/core/java/android/bluetooth/BluetoothProfile.java
+    """
+    PRIORITY_AUTO_CONNECT = 1000
+    PRIORITY_ON = 100
+    PRIORITY_OFF = 0
+    PRIORITY_UNDEFINED = -1
 
 
 class LogType(enum.Enum):
-  """Enumeration of device log type."""
-  DEFAULT_VALUE = 'GENERIC'
-  BLUETOOTH_DEVICE_SIMULATOR = 'BDS'
-  ICLEVER_HB01 = 'GENERIC'
+    """Enumeration of device log type."""
+    DEFAULT_VALUE = 'GENERIC'
+    BLUETOOTH_DEVICE_SIMULATOR = 'BDS'
+    ICLEVER_HB01 = 'GENERIC'
 
 
 class CallState(enum.IntEnum):
-  """Enum class for phone call state."""
-  IDLE = 0
-  RINGING = 1
-  OFFHOOK = 2
+    """Enum class for phone call state."""
+    IDLE = 0
+    RINGING = 1
+    OFFHOOK = 2
 
 
 class CallLogType(enum.IntEnum):
-  """Enum class for phone call log type."""
-  INCOMING_CALL = 1
-  OUTGOING_CALL = 2
-  MISSED_CALL = 3
+    """Enum class for phone call log type."""
+    INCOMING_CALL = 1
+    OUTGOING_CALL = 2
+    MISSED_CALL = 3
 
 
 class BluetoothA2dpCodec(enum.IntEnum):
-  """Enum class for Bluetooth A2DP codec type."""
-  SBC = 0
-  AAC = 1
-  APTX = 2
-  APTX_HD = 3
-  LDAC = 4
+    """Enum class for Bluetooth A2DP codec type."""
+    SBC = 0
+    AAC = 1
+    APTX = 2
+    APTX_HD = 3
+    LDAC = 4
diff --git a/system/blueberry/utils/bt_gatt_constants.py b/system/blueberry/utils/bt_gatt_constants.py
new file mode 100644
index 0000000..2067ddb
--- /dev/null
+++ b/system/blueberry/utils/bt_gatt_constants.py
@@ -0,0 +1,353 @@
+#!/usr/bin/env python3
+#
+# Copyright (C) 2022 The Android Open Source Project
+#
+# 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.
+
+import enum
+import sys
+
+
+def _import_str_enum():
+    # StrEnum is only introduced in Python 3.11
+    if sys.version_info >= (3, 11):
+        from enum import StrEnum
+        return StrEnum
+    else:
+        from typing import Type, TypeVar
+        _T = TypeVar("_T")
+
+        class StrEnumInternal(str, enum.Enum):
+            pass
+
+        return StrEnumInternal
+
+
+StrEnum = _import_str_enum()
+
+
+# Gatt Callback error messages
+class GattCallbackError(StrEnum):
+    CHAR_WRITE_REQ_ERR = "Characteristic Write Request event not found. Expected {}"
+    CHAR_WRITE_ERR = "Characteristic Write event not found. Expected {}"
+    DESC_WRITE_REQ_ERR = "Descriptor Write Request event not found. Expected {}"
+    DESC_WRITE_ERR = "Descriptor Write event not found. Expected {}"
+    CHAR_READ_ERR = "Characteristic Read event not found. Expected {}"
+    CHAR_READ_REQ_ERR = "Characteristic Read Request not found. Expected {}"
+    DESC_READ_ERR = "Descriptor Read event not found. Expected {}"
+    DESC_READ_REQ_ERR = "Descriptor Read Request event not found. Expected {}"
+    RD_REMOTE_RSSI_ERR = "Read Remote RSSI event not found. Expected {}"
+    GATT_SERV_DISC_ERR = "GATT Services Discovered event not found. Expected {}"
+    SERV_ADDED_ERR = "Service Added event not found. Expected {}"
+    MTU_CHANGED_ERR = "MTU Changed event not found. Expected {}"
+    MTU_SERV_CHANGED_ERR = "MTU Server Changed event not found. Expected {}"
+    GATT_CONN_CHANGE_ERR = "GATT Connection Changed event not found. Expected {}"
+    CHAR_CHANGE_ERR = "GATT Characteristic Changed event not fond. Expected {}"
+    PHY_READ_ERR = "Phy Read event not fond. Expected {}"
+    PHY_UPDATE_ERR = "Phy Update event not fond. Expected {}"
+    EXEC_WRITE_ERR = "GATT Execute Write event not found. Expected {}"
+
+
+# GATT callback strings as defined in GattClientFacade.java and
+# GattServerFacade.java implemented callbacks.
+class GattCallbackString(StrEnum):
+    CHAR_WRITE_REQ = "GattServer{}onCharacteristicWriteRequest"
+    EXEC_WRITE = "GattServer{}onExecuteWrite"
+    CHAR_WRITE = "GattConnect{}onCharacteristicWrite"
+    DESC_WRITE_REQ = "GattServer{}onDescriptorWriteRequest"
+    DESC_WRITE = "GattConnect{}onDescriptorWrite"
+    CHAR_READ = "GattConnect{}onCharacteristicRead"
+    CHAR_READ_REQ = "GattServer{}onCharacteristicReadRequest"
+    DESC_READ = "GattConnect{}onDescriptorRead"
+    DESC_READ_REQ = "GattServer{}onDescriptorReadRequest"
+    RD_REMOTE_RSSI = "GattConnect{}onReadRemoteRssi"
+    GATT_SERV_DISC = "GattConnect{}onServicesDiscovered"
+    SERV_ADDED = "GattServer{}onServiceAdded"
+    MTU_CHANGED = "GattConnect{}onMtuChanged"
+    MTU_SERV_CHANGED = "GattServer{}onMtuChanged"
+    GATT_CONN_CHANGE = "GattConnect{}onConnectionStateChange"
+    CHAR_CHANGE = "GattConnect{}onCharacteristicChanged"
+    PHY_READ = "GattConnect{}onPhyRead"
+    PHY_UPDATE = "GattConnect{}onPhyUpdate"
+    SERV_PHY_READ = "GattServer{}onPhyRead"
+    SERV_PHY_UPDATE = "GattServer{}onPhyUpdate"
+
+
+# yapf: disable
+# GATT event dictionary of expected callbacks and errors.
+class GattEvent(dict, enum.Enum):
+
+    def __getitem__(self, item):
+        return self._value_[item]
+
+    CHAR_WRITE_REQ = {
+            "evt": GattCallbackString.CHAR_WRITE_REQ,
+            "err": GattCallbackError.CHAR_WRITE_REQ_ERR
+    }
+    EXEC_WRITE = {
+            "evt": GattCallbackString.EXEC_WRITE,
+            "err": GattCallbackError.EXEC_WRITE_ERR
+    }
+    CHAR_WRITE = {
+            "evt": GattCallbackString.CHAR_WRITE,
+            "err": GattCallbackError.CHAR_WRITE_ERR
+    }
+    DESC_WRITE_REQ = {
+            "evt": GattCallbackString.DESC_WRITE_REQ,
+            "err": GattCallbackError.DESC_WRITE_REQ_ERR
+    }
+    DESC_WRITE = {
+            "evt": GattCallbackString.DESC_WRITE,
+            "err": GattCallbackError.DESC_WRITE_ERR
+    }
+    CHAR_READ = {
+            "evt": GattCallbackString.CHAR_READ,
+            "err": GattCallbackError.CHAR_READ_ERR
+    }
+    CHAR_READ_REQ = {
+            "evt": GattCallbackString.CHAR_READ_REQ,
+            "err": GattCallbackError.CHAR_READ_REQ_ERR
+    }
+    DESC_READ = {
+            "evt": GattCallbackString.DESC_READ,
+            "err": GattCallbackError.DESC_READ_ERR
+    }
+    DESC_READ_REQ = {
+            "evt": GattCallbackString.DESC_READ_REQ,
+            "err": GattCallbackError.DESC_READ_REQ_ERR
+    }
+    RD_REMOTE_RSSI = {
+            "evt": GattCallbackString.RD_REMOTE_RSSI,
+            "err": GattCallbackError.RD_REMOTE_RSSI_ERR
+    }
+    GATT_SERV_DISC = {
+            "evt": GattCallbackString.GATT_SERV_DISC,
+            "err": GattCallbackError.GATT_SERV_DISC_ERR
+    }
+    SERV_ADDED = {
+            "evt": GattCallbackString.SERV_ADDED,
+            "err": GattCallbackError.SERV_ADDED_ERR
+    }
+    MTU_CHANGED = {
+            "evt": GattCallbackString.MTU_CHANGED,
+            "err": GattCallbackError.MTU_CHANGED_ERR
+    }
+    GATT_CONN_CHANGE = {
+            "evt": GattCallbackString.GATT_CONN_CHANGE,
+            "err": GattCallbackError.GATT_CONN_CHANGE_ERR
+    }
+    CHAR_CHANGE = {
+            "evt": GattCallbackString.CHAR_CHANGE,
+            "err": GattCallbackError.CHAR_CHANGE_ERR
+    }
+    PHY_READ = {
+            "evt": GattCallbackString.PHY_READ,
+            "err": GattCallbackError.PHY_READ_ERR
+    }
+    PHY_UPDATE = {
+            "evt": GattCallbackString.PHY_UPDATE,
+            "err": GattCallbackError.PHY_UPDATE_ERR
+    }
+    SERV_PHY_READ = {
+            "evt": GattCallbackString.SERV_PHY_READ,
+            "err": GattCallbackError.PHY_READ_ERR
+    }
+    SERV_PHY_UPDATE = {
+            "evt": GattCallbackString.SERV_PHY_UPDATE,
+            "err": GattCallbackError.PHY_UPDATE_ERR
+    }
+# yapf: enable
+
+
+# Matches constants of connection states defined in BluetoothGatt.java
+class GattConnectionState(enum.IntEnum):
+    STATE_DISCONNECTED = 0
+    STATE_CONNECTING = 1
+    STATE_CONNECTED = 2
+    STATE_DISCONNECTING = 3
+
+
+# Matches constants of Bluetooth GATT Characteristic values as defined
+# in BluetoothGattCharacteristic.java
+class GattCharacteristic(enum.IntEnum):
+    PROPERTY_BROADCAST = 0x01
+    PROPERTY_READ = 0x02
+    PROPERTY_WRITE_NO_RESPONSE = 0x04
+    PROPERTY_WRITE = 0x08
+    PROPERTY_NOTIFY = 0x10
+    PROPERTY_INDICATE = 0x20
+    PROPERTY_SIGNED_WRITE = 0x40
+    PROPERTY_EXTENDED_PROPS = 0x80
+    PERMISSION_READ = 0x01
+    PERMISSION_READ_ENCRYPTED = 0x02
+    PERMISSION_READ_ENCRYPTED_MITM = 0x04
+    PERMISSION_WRITE = 0x10
+    PERMISSION_WRITE_ENCRYPTED = 0x20
+    PERMISSION_WRITE_ENCRYPTED_MITM = 0x40
+    PERMISSION_WRITE_SIGNED = 0x80
+    PERMISSION_WRITE_SIGNED_MITM = 0x100
+    WRITE_TYPE_DEFAULT = 0x02
+    WRITE_TYPE_NO_RESPONSE = 0x01
+    WRITE_TYPE_SIGNED = 0x04
+    FORMAT_UINT8 = 0x11
+    FORMAT_UINT16 = 0x12
+    FORMAT_UINT32 = 0x14
+    FORMAT_SINT8 = 0x21
+    FORMAT_SINT16 = 0x22
+    FORMAT_SINT32 = 0x24
+    FORMAT_SFLOAT = 0x32
+    FORMAT_FLOAT = 0x34
+
+
+# Matches constants of Bluetooth GATT Characteristic values as defined
+# in BluetoothGattDescriptor.java
+class GattDescriptor(list, enum.Enum):
+
+    def __getitem__(self, item):
+        return self._value_[item]
+
+    ENABLE_NOTIFICATION_VALUE = [0x01, 0x00]
+    ENABLE_INDICATION_VALUE = [0x02, 0x00]
+    DISABLE_NOTIFICATION_VALUE = [0x00, 0x00]
+    PERMISSION_READ = [0x01]
+    PERMISSION_READ_ENCRYPTED = [0x02]
+    PERMISSION_READ_ENCRYPTED_MITM = [0x04]
+    PERMISSION_WRITE = [0x10]
+    PERMISSION_WRITE_ENCRYPTED = [0x20]
+    PERMISSION_WRITE_ENCRYPTED_MITM = [0x40]
+    PERMISSION_WRITE_SIGNED = [0x80]
+    PERMISSION_WRITE_SIGNED_MITM = [0x100]
+
+
+# https://www.bluetooth.com/specifications/gatt/descriptors
+class GattCharDesc(StrEnum):
+    GATT_CHARAC_EXT_PROPER_UUID = '00002900-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_USER_DESC_UUID = '00002901-0000-1000-8000-00805f9b34fb'
+    GATT_CLIENT_CHARAC_CFG_UUID = '00002902-0000-1000-8000-00805f9b34fb'
+    GATT_SERVER_CHARAC_CFG_UUID = '00002903-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_FMT_UUID = '00002904-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_AGREG_FMT_UUID = '00002905-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_VALID_RANGE_UUID = '00002906-0000-1000-8000-00805f9b34fb'
+    GATT_EXTERNAL_REPORT_REFERENCE = '00002907-0000-1000-8000-00805f9b34fb'
+    GATT_REPORT_REFERENCE = '00002908-0000-1000-8000-00805f9b34fb'
+
+
+# https://www.bluetooth.com/specifications/gatt/characteristics
+class GattCharTypes(StrEnum):
+    GATT_CHARAC_DEVICE_NAME = '00002a00-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_APPEARANCE = '00002a01-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_PERIPHERAL_PRIV_FLAG = '00002a02-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_RECONNECTION_ADDRESS = '00002a03-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_PERIPHERAL_PREF_CONN = '00002a04-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_SERVICE_CHANGED = '00002a05-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_SYSTEM_ID = '00002a23-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_MODEL_NUMBER_STRING = '00002a24-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_SERIAL_NUMBER_STRING = '00002a25-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_FIRMWARE_REVISION_STRING = '00002a26-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_HARDWARE_REVISION_STRING = '00002a27-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_SOFTWARE_REVISION_STRING = '00002a28-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_MANUFACTURER_NAME_STRING = '00002a29-0000-1000-8000-00805f9b34fb'
+    GATT_CHARAC_PNP_ID = '00002a50-0000-1000-8000-00805f9b34fb'
+
+
+# Matches constants of Bluetooth GATT Characteristic values as defined
+# in BluetoothGattCharacteristic.java
+class CharacteristicValueFormat(enum.Enum):
+    STRING = 0x1
+    BYTE = 0x2
+    FORMAT_SINT8 = 0x21
+    FORMAT_UINT8 = 0x11
+    FORMAT_SINT16 = 0x22
+    FORMAT_UINT16 = 0x12
+    FORMAT_SINT32 = 0x24
+    FORMAT_UINT32 = 0x14
+
+
+# Matches constants of Bluetooth Gatt Service types as defined in
+# BluetoothGattService.java
+class GattServiceType(enum.IntEnum):
+    SERVICE_TYPE_PRIMARY = 0
+    SERVICE_TYPE_SECONDARY = 1
+
+
+# Matches constants of Bluetooth Gatt Connection Priority values as defined in
+# BluetoothGatt.java
+class GattConnectionPriority(enum.IntEnum):
+    CONNECTION_PRIORITY_BALANCED = 0
+    CONNECTION_PRIORITY_HIGH = 1
+    CONNECTION_PRIORITY_LOW_POWER = 2
+
+
+# Min and max MTU values
+class GattMtuSize(enum.IntEnum):
+    MIN = 23
+    MAX = 217
+
+
+# Gatt Characteristic attribute lengths
+class GattCharacteristicAttrLength(enum.IntEnum):
+    MTU_ATTR_1 = 1
+    MTU_ATTR_2 = 3
+    MTU_ATTR_3 = 15
+
+
+# Matches constants of Bluetooth Gatt operations status as defined in
+# BluetoothGatt.java
+class BluetoothGatt(enum.IntEnum):
+    GATT_SUCCESS = 0
+    GATT_FAILURE = 0x101
+
+
+# Matches constants of Bluetooth transport values as defined in
+# BluetoothDevice.java
+class GattTransport(enum.IntEnum):
+    TRANSPORT_AUTO = 0x00
+    TRANSPORT_BREDR = 0x01
+    TRANSPORT_LE = 0x02
+
+
+# Matches constants of Bluetooth physical channeling values as defined in
+# BluetoothDevice.java
+class GattPhy(enum.IntEnum):
+    PHY_LE_1M = 1
+    PHY_LE_2M = 2
+    PHY_LE_CODED = 3
+
+
+# Matches constants of Bluetooth physical channeling bitmask values as defined
+# in BluetoothDevice.java
+class GattPhyMask(enum.IntEnum):
+    PHY_LE_1M_MASK = 1
+    PHY_LE_2M_MASK = 2
+    PHY_LE_CODED_MASK = 4
+
+
+# Values as defined in the Bluetooth GATT specification
+GattServerResponses = {
+    "GATT_SUCCESS": 0x0,
+    "GATT_FAILURE": 0x1,
+    "GATT_READ_NOT_PERMITTED": 0x2,
+    "GATT_WRITE_NOT_PERMITTED": 0x3,
+    "GATT_INVALID_PDU": 0x4,
+    "GATT_INSUFFICIENT_AUTHENTICATION": 0x5,
+    "GATT_REQUEST_NOT_SUPPORTED": 0x6,
+    "GATT_INVALID_OFFSET": 0x7,
+    "GATT_INSUFFICIENT_AUTHORIZATION": 0x8,
+    "GATT_INVALID_ATTRIBUTE_LENGTH": 0xd,
+    "GATT_INSUFFICIENT_ENCRYPTION": 0xf,
+    "GATT_CONNECTION_CONGESTED": 0x8f,
+    "GATT_13_ERR": 0x13,
+    "GATT_12_ERR": 0x12,
+    "GATT_0C_ERR": 0x0C,
+    "GATT_16": 0x16
+}
diff --git a/system/blueberry/utils/bt_gatt_utils.py b/system/blueberry/utils/bt_gatt_utils.py
new file mode 100644
index 0000000..4ad42eb
--- /dev/null
+++ b/system/blueberry/utils/bt_gatt_utils.py
@@ -0,0 +1,349 @@
+#!/usr/bin/env python3
+#
+# Copyright (C) 2016 The Android Open Source Project
+#
+# 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.
+"""
+Original file:
+    tools/test/connectivity/acts_tests/acts_contrib/test_utils/bt/bt_gatt_utils.py
+"""
+
+import logging
+import pprint
+from queue import Empty
+
+from blueberry.utils.bt_gatt_constants import GattCallbackError
+from blueberry.utils.bt_gatt_constants import GattCallbackString
+from blueberry.utils.bt_gatt_constants import GattCharacteristic
+from blueberry.utils.bt_gatt_constants import GattConnectionState
+from blueberry.utils.bt_gatt_constants import GattDescriptor
+from blueberry.utils.bt_gatt_constants import GattPhyMask
+from blueberry.utils.bt_gatt_constants import GattServiceType
+from blueberry.utils.bt_gatt_constants import GattTransport
+from blueberry.utils.bt_test_utils import BtTestUtilsError
+from blueberry.utils.bt_test_utils import get_mac_address_of_generic_advertisement
+from mobly.controllers.android_device import AndroidDevice
+from mobly.controllers.android_device_lib.event_dispatcher import EventDispatcher
+from mobly.controllers.android_device_lib.sl4a_client import Sl4aClient
+
+default_timeout = 10
+log = logging
+
+
+class GattTestUtilsError(Exception):
+    pass
+
+
+def setup_gatt_connection(central: AndroidDevice,
+                          mac_address,
+                          autoconnect,
+                          transport=GattTransport.TRANSPORT_AUTO,
+                          opportunistic=False):
+    gatt_callback = central.sl4a.gattCreateGattCallback()
+    log.info("Gatt Connect to mac address {}.".format(mac_address))
+    bluetooth_gatt = central.sl4a.gattClientConnectGatt(gatt_callback, mac_address, autoconnect, transport,
+                                                        opportunistic, GattPhyMask.PHY_LE_1M_MASK)
+    expected_event = GattCallbackString.GATT_CONN_CHANGE.format(gatt_callback)
+    try:
+        event = central.ed.pop_event(expected_event, default_timeout)
+    except Empty:
+        close_gatt_client(central, bluetooth_gatt)
+        raise GattTestUtilsError("Could not establish a connection to "
+                                 "peripheral. Expected event: {}".format(expected_event))
+    if event['data']['State'] != GattConnectionState.STATE_CONNECTED:
+        close_gatt_client(central, bluetooth_gatt)
+        try:
+            central.sl4a.gattClientClose(bluetooth_gatt)
+        except Exception:
+            logging.debug("Failed to close gatt client.")
+        raise GattTestUtilsError("Could not establish a connection to "
+                                 "peripheral. Event Details: {}".format(pprint.pformat(event)))
+    return bluetooth_gatt, gatt_callback
+
+
+def close_gatt_client(central: AndroidDevice, bluetooth_gatt):
+    try:
+        central.sl4a.gattClientClose(bluetooth_gatt)
+    except Exception:
+        log.debug("Failed to close gatt client.")
+
+
+def disconnect_gatt_connection(central: AndroidDevice, bluetooth_gatt, gatt_callback):
+    central.sl4a.gattClientDisconnect(bluetooth_gatt)
+    wait_for_gatt_disconnect_event(central, gatt_callback)
+    return
+
+
+def wait_for_gatt_disconnect_event(central: AndroidDevice, gatt_callback):
+    expected_event = GattCallbackString.GATT_CONN_CHANGE.format(gatt_callback)
+    try:
+        event = central.ed.pop_event(expected_event, default_timeout)
+    except Empty:
+        raise GattTestUtilsError(GattCallbackError.GATT_CONN_CHANGE_ERR.format(expected_event))
+    found_state = event['data']['State']
+    expected_state = GattConnectionState.STATE_DISCONNECTED
+    if found_state != expected_state:
+        raise GattTestUtilsError("GATT connection state change expected {}, found {}".format(
+            expected_event, found_state))
+    return
+
+
+def orchestrate_gatt_connection(central: AndroidDevice,
+                                peripheral: AndroidDevice,
+                                transport=GattTransport.TRANSPORT_LE,
+                                mac_address=None,
+                                autoconnect=False,
+                                opportunistic=False):
+    adv_callback = None
+    if mac_address is None:
+        if transport == GattTransport.TRANSPORT_LE:
+            try:
+                mac_address, adv_callback, scan_callback = (get_mac_address_of_generic_advertisement(
+                    central, peripheral))
+            except BtTestUtilsError as err:
+                raise GattTestUtilsError("Error in getting mac address: {}".format(err))
+        else:
+            mac_address = peripheral.sl4a.bluetoothGetLocalAddress()
+            adv_callback = None
+    bluetooth_gatt, gatt_callback = setup_gatt_connection(central, mac_address, autoconnect, transport, opportunistic)
+    return bluetooth_gatt, gatt_callback, adv_callback
+
+
+def run_continuous_write_descriptor(cen_droid: Sl4aClient,
+                                    cen_ed: EventDispatcher,
+                                    per_droid: Sl4aClient,
+                                    per_ed: EventDispatcher,
+                                    gatt_server,
+                                    gatt_server_callback,
+                                    bluetooth_gatt,
+                                    services_count,
+                                    discovered_services_index,
+                                    number_of_iterations=100000):
+    log.info("Starting continuous write")
+    bt_device_id = 0
+    status = 1
+    offset = 1
+    test_value = [1, 2, 3, 4, 5, 6, 7]
+    test_value_return = [1, 2, 3]
+    for _ in range(number_of_iterations):
+        try:
+            for i in range(services_count):
+                characteristic_uuids = (cen_droid.gattClientGetDiscoveredCharacteristicUuids(
+                    discovered_services_index, i))
+                log.info(characteristic_uuids)
+                for characteristic in characteristic_uuids:
+                    descriptor_uuids = (cen_droid.gattClientGetDiscoveredDescriptorUuids(
+                        discovered_services_index, i, characteristic))
+                    log.info(descriptor_uuids)
+                    for descriptor in descriptor_uuids:
+                        cen_droid.gattClientDescriptorSetValue(bluetooth_gatt, discovered_services_index, i,
+                                                               characteristic, descriptor, test_value)
+                        cen_droid.gattClientWriteDescriptor(bluetooth_gatt, discovered_services_index, i,
+                                                            characteristic, descriptor)
+                        expected_event = \
+                            GattCallbackString.DESC_WRITE_REQ.format(
+                                gatt_server_callback)
+                        try:
+                            event = per_ed.pop_event(expected_event, default_timeout)
+                        except Empty:
+                            log.error(GattCallbackError.DESC_WRITE_REQ_ERR.format(expected_event))
+                            return False
+                        request_id = event['data']['requestId']
+                        found_value = event['data']['value']
+                        if found_value != test_value:
+                            log.error("Values didn't match. Found: {}, Expected: " "{}".format(found_value, test_value))
+                        per_droid.gattServerSendResponse(gatt_server, bt_device_id, request_id, status, offset,
+                                                         test_value_return)
+                        expected_event = GattCallbackString.DESC_WRITE.format(bluetooth_gatt)
+                        try:
+                            cen_ed.pop_event(expected_event, default_timeout)
+                        except Empty:
+                            log.error(GattCallbackError.DESC_WRITE_ERR.format(expected_event))
+                            raise Exception("Thread ended prematurely.")
+        except Exception as err:
+            log.error("Continuing but found exception: {}".format(err))
+
+
+def setup_characteristics_and_descriptors_read_write(droid: Sl4aClient):
+    characteristic_input = [
+        {
+            'uuid': "aa7edd5a-4d1d-4f0e-883a-d145616a1630",
+            'property': GattCharacteristic.PROPERTY_WRITE | GattCharacteristic.PROPERTY_WRITE_NO_RESPONSE,
+            'permission': GattCharacteristic.PERMISSION_WRITE
+        },
+        {
+            'uuid': "21c0a0bf-ad51-4a2d-8124-b74003e4e8c8",
+            'property': GattCharacteristic.PROPERTY_WRITE | GattCharacteristic.PROPERTY_READ,
+            'permission': GattCharacteristic.PERMISSION_READ
+        },
+        {
+            'uuid': "6774191f-6ec3-4aa2-b8a8-cf830e41fda6",
+            'property': GattCharacteristic.PROPERTY_NOTIFY | GattCharacteristic.PROPERTY_READ,
+            'permission': GattCharacteristic.PERMISSION_READ
+        },
+    ]
+    descriptor_input = [{
+        'uuid': "aa7edd5a-4d1d-4f0e-883a-d145616a1630",
+        'property': GattDescriptor.PERMISSION_READ[0] | GattDescriptor.PERMISSION_WRITE[0],
+    }, {
+        'uuid': "76d5ed92-ca81-4edb-bb6b-9f019665fb32",
+        'property': GattDescriptor.PERMISSION_READ[0] | GattDescriptor.PERMISSION_WRITE[0],
+    }]
+    characteristic_list = setup_gatt_characteristics(droid, characteristic_input)
+    descriptor_list = setup_gatt_descriptors(droid, descriptor_input)
+    return characteristic_list, descriptor_list
+
+
+def setup_multiple_services(peripheral: AndroidDevice):
+    per_droid, per_ed = peripheral.sl4a, peripheral.sl4a.ed
+    gatt_server_callback = per_droid.gattServerCreateGattServerCallback()
+    gatt_server = per_droid.gattServerOpenGattServer(gatt_server_callback)
+    characteristic_list, descriptor_list = (setup_characteristics_and_descriptors_read_write(per_droid))
+    per_droid.gattServerCharacteristicAddDescriptor(characteristic_list[1], descriptor_list[0])
+    per_droid.gattServerCharacteristicAddDescriptor(characteristic_list[2], descriptor_list[1])
+    gattService = per_droid.gattServerCreateService("00000000-0000-1000-8000-00805f9b34fb",
+                                                    GattServiceType.SERVICE_TYPE_PRIMARY)
+    gattService2 = per_droid.gattServerCreateService("FFFFFFFF-0000-1000-8000-00805f9b34fb",
+                                                     GattServiceType.SERVICE_TYPE_PRIMARY)
+    gattService3 = per_droid.gattServerCreateService("3846D7A0-69C8-11E4-BA00-0002A5D5C51B",
+                                                     GattServiceType.SERVICE_TYPE_PRIMARY)
+    for characteristic in characteristic_list:
+        per_droid.gattServerAddCharacteristicToService(gattService, characteristic)
+    per_droid.gattServerAddService(gatt_server, gattService)
+    expected_event = GattCallbackString.SERV_ADDED.format(gatt_server_callback)
+    try:
+        per_ed.pop_event(expected_event, default_timeout)
+    except Empty:
+        peripheral.sl4a.gattServerClose(gatt_server)
+        raise GattTestUtilsError(GattCallbackError.SERV_ADDED_ERR.format(expected_event))
+    for characteristic in characteristic_list:
+        per_droid.gattServerAddCharacteristicToService(gattService2, characteristic)
+    per_droid.gattServerAddService(gatt_server, gattService2)
+    try:
+        per_ed.pop_event(expected_event, default_timeout)
+    except Empty:
+        peripheral.sl4a.gattServerClose(gatt_server)
+        raise GattTestUtilsError(GattCallbackError.SERV_ADDED_ERR.format(expected_event))
+    for characteristic in characteristic_list:
+        per_droid.gattServerAddCharacteristicToService(gattService3, characteristic)
+    per_droid.gattServerAddService(gatt_server, gattService3)
+    try:
+        per_ed.pop_event(expected_event, default_timeout)
+    except Empty:
+        peripheral.sl4a.gattServerClose(gatt_server)
+        raise GattTestUtilsError(GattCallbackError.SERV_ADDED_ERR.format(expected_event))
+    return gatt_server_callback, gatt_server
+
+
+def setup_characteristics_and_descriptors_notify_read(droid: Sl4aClient):
+    characteristic_input = [
+        {
+            'uuid': "aa7edd5a-4d1d-4f0e-883a-d145616a1630",
+            'property': GattCharacteristic.PROPERTY_WRITE | GattCharacteristic.PROPERTY_WRITE_NO_RESPONSE,
+            'permission': GattCharacteristic.PROPERTY_WRITE
+        },
+        {
+            'uuid': "21c0a0bf-ad51-4a2d-8124-b74003e4e8c8",
+            'property': GattCharacteristic.PROPERTY_NOTIFY | GattCharacteristic.PROPERTY_READ,
+            'permission': GattCharacteristic.PERMISSION_READ
+        },
+        {
+            'uuid': "6774191f-6ec3-4aa2-b8a8-cf830e41fda6",
+            'property': GattCharacteristic.PROPERTY_NOTIFY | GattCharacteristic.PROPERTY_READ,
+            'permission': GattCharacteristic.PERMISSION_READ
+        },
+    ]
+    descriptor_input = [{
+        'uuid': "aa7edd5a-4d1d-4f0e-883a-d145616a1630",
+        'property': GattDescriptor.PERMISSION_READ[0] | GattDescriptor.PERMISSION_WRITE[0],
+    }, {
+        'uuid': "76d5ed92-ca81-4edb-bb6b-9f019665fb32",
+        'property': GattDescriptor.PERMISSION_READ[0] | GattDescriptor.PERMISSION_WRITE[0],
+    }]
+    characteristic_list = setup_gatt_characteristics(droid, characteristic_input)
+    descriptor_list = setup_gatt_descriptors(droid, descriptor_input)
+    return characteristic_list, descriptor_list
+
+
+def setup_gatt_characteristics(droid: Sl4aClient, input):
+    characteristic_list = []
+    for item in input:
+        index = droid.gattServerCreateBluetoothGattCharacteristic(item['uuid'], item['property'], item['permission'])
+        characteristic_list.append(index)
+    return characteristic_list
+
+
+def setup_gatt_descriptors(droid: Sl4aClient, input):
+    descriptor_list = []
+    for item in input:
+        index = droid.gattServerCreateBluetoothGattDescriptor(
+            item['uuid'],
+            item['property'],
+        )
+        descriptor_list.append(index)
+    log.info("setup descriptor list: {}".format(descriptor_list))
+    return descriptor_list
+
+
+def setup_gatt_mtu(central: AndroidDevice, bluetooth_gatt, gatt_callback, mtu):
+    """utility function to set mtu for GATT connection.
+
+    Steps:
+    1. Request mtu change.
+    2. Check if the mtu is changed to the new value
+
+    Args:
+        central: test device for client to scan.
+        bluetooth_gatt: GATT object
+        mtu: new mtu value to be set
+
+    Returns:
+        If success, return True.
+        if fail, return False
+    """
+    central.sl4a.gattClientRequestMtu(bluetooth_gatt, mtu)
+    expected_event = GattCallbackString.MTU_CHANGED.format(gatt_callback)
+    try:
+        mtu_event = central.ed.pop_event(expected_event, default_timeout)
+        mtu_size_found = mtu_event['data']['MTU']
+        if mtu_size_found != mtu:
+            log.error("MTU size found: {}, expected: {}".format(mtu_size_found, mtu))
+            return False
+    except Empty:
+        log.error(GattCallbackError.MTU_CHANGED_ERR.format(expected_event))
+        return False
+    return True
+
+
+def log_gatt_server_uuids(central: AndroidDevice, discovered_services_index, bluetooth_gatt=None):
+    services_count = central.sl4a.gattClientGetDiscoveredServicesCount(discovered_services_index)
+    for i in range(services_count):
+        service = central.sl4a.gattClientGetDiscoveredServiceUuid(discovered_services_index, i)
+        log.info("Discovered service uuid {}".format(service))
+        characteristic_uuids = (central.sl4a.gattClientGetDiscoveredCharacteristicUuids(discovered_services_index, i))
+        for j in range(len(characteristic_uuids)):
+            descriptor_uuids = (central.sl4a.gattClientGetDiscoveredDescriptorUuidsByIndex(
+                discovered_services_index, i, j))
+            if bluetooth_gatt:
+                char_inst_id = central.sl4a.gattClientGetCharacteristicInstanceId(bluetooth_gatt,
+                                                                                  discovered_services_index, i, j)
+                log.info("Discovered characteristic handle uuid: {} {}".format(
+                    hex(char_inst_id), characteristic_uuids[j]))
+                for k in range(len(descriptor_uuids)):
+                    desc_inst_id = central.sl4a.gattClientGetDescriptorInstanceId(bluetooth_gatt,
+                                                                                  discovered_services_index, i, j, k)
+                    log.info("Discovered descriptor handle uuid: {} {}".format(hex(desc_inst_id), descriptor_uuids[k]))
+            else:
+                log.info("Discovered characteristic uuid: {}".format(characteristic_uuids[j]))
+                for k in range(len(descriptor_uuids)):
+                    log.info("Discovered descriptor uuid {}".format(descriptor_uuids[k]))
diff --git a/system/blueberry/utils/bt_test_utils.py b/system/blueberry/utils/bt_test_utils.py
index bfd184d..c1948e7 100644
--- a/system/blueberry/utils/bt_test_utils.py
+++ b/system/blueberry/utils/bt_test_utils.py
@@ -13,21 +13,33 @@
 import random
 import string
 import time
-from typing import Optional
 import wave
+from queue import Empty
+from typing import Optional
+
+from blueberry.tests.gd_sl4a.lib.ble_lib import generate_ble_advertise_objects
+from blueberry.tests.gd_sl4a.lib.bt_constants import adv_succ
+from blueberry.tests.gd_sl4a.lib.bt_constants import ble_advertise_settings_modes
+from blueberry.tests.gd_sl4a.lib.bt_constants import ble_advertise_settings_tx_powers
+from blueberry.tests.gd_sl4a.lib.bt_constants import bt_default_timeout
+from mobly.controllers.android_device import AndroidDevice
+
+
+class BtTestUtilsError(Exception):
+    pass
 
 
 def convert_pcm_to_wav(pcm_file_path, wave_file_path, audio_params):
-  """Converts raw pcm data into wave file.
+    """Converts raw pcm data into wave file.
 
-  Args:
-      pcm_file_path: File path of origin pcm file.
-      wave_file_path: File path of converted wave file.
-      audio_params: A dict with audio configuration.
-  """
-  with open(pcm_file_path, 'rb') as pcm_file:
-    frames = pcm_file.read()
-  write_record_file(wave_file_path, audio_params, frames)
+    Args:
+        pcm_file_path: File path of origin pcm file.
+        wave_file_path: File path of converted wave file.
+        audio_params: A dict with audio configuration.
+    """
+    with open(pcm_file_path, 'rb') as pcm_file:
+        frames = pcm_file.read()
+    write_record_file(wave_file_path, audio_params, frames)
 
 
 def create_vcf_from_vcard(output_path: str,
@@ -35,167 +47,223 @@
                           first_name: Optional[str] = None,
                           last_name: Optional[str] = None,
                           phone_number: Optional[int] = None) -> str:
-  """Creates a vcf file from vCard.
+    """Creates a vcf file from vCard.
 
-  Args:
-    output_path: Path of the output vcf file.
-    num_of_contacts: Number of contacts to be generated.
-    first_name: First name of the contacts.
-    last_name: Last name of the contacts.
-    phone_number: Phone number of the contacts.
+    Args:
+        output_path: Path of the output vcf file.
+        num_of_contacts: Number of contacts to be generated.
+        first_name: First name of the contacts.
+        last_name: Last name of the contacts.
+        phone_number: Phone number of the contacts.
 
-  Returns:
-    vcf_file_path: Path of the output vcf file. E.g.
-        "/<output_path>/contacts_<time>.vcf".
-  """
-  file_name = f'contacts_{int(time.time())}.vcf'
-  vcf_file_path = os.path.join(output_path, file_name)
-  with open(vcf_file_path, 'w+') as f:
-    for i in range(num_of_contacts):
-      lines = []
-      if first_name is None:
-        first_name = 'Person'
-      vcard_last_name = last_name
-      if last_name is None:
-        vcard_last_name = i
-      vcard_phone_number = phone_number
-      if phone_number is None:
-        vcard_phone_number = random.randrange(int(10e10))
-      lines.append('BEGIN:VCARD\n')
-      lines.append('VERSION:2.1\n')
-      lines.append(f'N:{vcard_last_name};{first_name};;;\n')
-      lines.append(f'FN:{first_name} {vcard_last_name}\n')
-      lines.append(f'TEL;CELL:{vcard_phone_number}\n')
-      lines.append(f'EMAIL;PREF:{first_name}{vcard_last_name}@gmail.com\n')
-      lines.append('END:VCARD\n')
-      f.write(''.join(lines))
-  return vcf_file_path
+    Returns:
+        vcf_file_path: Path of the output vcf file. E.g.
+            "/<output_path>/contacts_<time>.vcf".
+    """
+    file_name = f'contacts_{int(time.time())}.vcf'
+    vcf_file_path = os.path.join(output_path, file_name)
+    with open(vcf_file_path, 'w+') as f:
+        for i in range(num_of_contacts):
+            lines = []
+            if first_name is None:
+                first_name = 'Person'
+            vcard_last_name = last_name
+            if last_name is None:
+                vcard_last_name = i
+            vcard_phone_number = phone_number
+            if phone_number is None:
+                vcard_phone_number = random.randrange(int(10e10))
+            lines.append('BEGIN:VCARD\n')
+            lines.append('VERSION:2.1\n')
+            lines.append(f'N:{vcard_last_name};{first_name};;;\n')
+            lines.append(f'FN:{first_name} {vcard_last_name}\n')
+            lines.append(f'TEL;CELL:{vcard_phone_number}\n')
+            lines.append(f'EMAIL;PREF:{first_name}{vcard_last_name}@gmail.com\n')
+            lines.append('END:VCARD\n')
+            f.write(''.join(lines))
+    return vcf_file_path
 
 
-def generate_id_by_size(size,
-                        chars=(string.ascii_lowercase + string.ascii_uppercase +
-                               string.digits)):
-  """Generate random ascii characters of input size and input char types.
+def generate_id_by_size(size, chars=(string.ascii_lowercase + string.ascii_uppercase + string.digits)):
+    """Generate random ascii characters of input size and input char types.
 
-  Args:
-      size: Input size of string.
-      chars: (Optional) Chars to use in generating a random string.
+    Args:
+        size: Input size of string.
+        chars: (Optional) Chars to use in generating a random string.
 
-  Returns:
-      String of random input chars at the input size.
-  """
-  return ''.join(random.choice(chars) for _ in range(size))
+    Returns:
+        String of random input chars at the input size.
+    """
+    return ''.join(random.choice(chars) for _ in range(size))
 
 
 def get_duration_seconds(wav_file_path):
-  """Get duration of most recently recorded file.
+    """Get duration of most recently recorded file.
 
-  Args:
-      wav_file_path: path of the wave file.
+    Args:
+        wav_file_path: path of the wave file.
 
-  Returns:
-      duration (float): duration of recorded file in seconds.
-  """
-  f = wave.open(wav_file_path, 'r')
-  frames = f.getnframes()
-  rate = f.getframerate()
-  duration = (frames / float(rate))
-  f.close()
-  return duration
+    Returns:
+        duration (float): duration of recorded file in seconds.
+    """
+    f = wave.open(wav_file_path, 'r')
+    frames = f.getnframes()
+    rate = f.getframerate()
+    duration = (frames / float(rate))
+    f.close()
+    return duration
 
 
-def wait_until(timeout_sec,
-               condition_func,
-               func_args,
-               expected_value,
-               exception=None,
-               interval_sec=0.5):
-  """Waits until a function returns a expected value or timeout is reached.
+def wait_until(timeout_sec, condition_func, func_args, expected_value, exception=None, interval_sec=0.5):
+    """Waits until a function returns a expected value or timeout is reached.
 
-  Example usage:
-    ```
-    def is_bluetooth_enabled(device) -> bool:
-      do something and return something...
+    Example usage:
+        ```
+        def is_bluetooth_enabled(device) -> bool:
+          do something and return something...
 
-    # Waits and checks if Bluetooth is turned on.
-    bt_test_utils.wait_until(
-        timeout_sec=10,
-        condition_func=is_bluetooth_enabled,
-        func_args=[dut],
-        expected_value=True,
-        exception=signals.TestFailure('Failed to turn on Bluetooth.'),
-        interval_sec=1)
-    ```
+        # Waits and checks if Bluetooth is turned on.
+        bt_test_utils.wait_until(
+            timeout_sec=10,
+            condition_func=is_bluetooth_enabled,
+            func_args=[dut],
+            expected_value=True,
+            exception=signals.TestFailure('Failed to turn on Bluetooth.'),
+            interval_sec=1)
+        ```
 
-  Args:
-    timeout_sec: float, max waiting time in seconds.
-    condition_func: function, when the condiction function returns the expected
-        value, the waiting mechanism will be interrupted.
-    func_args: tuple or list, the arguments for the condition function.
-    expected_value: a expected value that the condition function returns.
-    exception: Exception, an exception will be raised when timed out if needed.
-    interval_sec: float, interval time between calls of the condition function
-        in seconds.
+    Args:
+        timeout_sec: float, max waiting time in seconds.
+        condition_func: function, when the condiction function returns the expected
+            value, the waiting mechanism will be interrupted.
+        func_args: tuple or list, the arguments for the condition function.
+        expected_value: a expected value that the condition function returns.
+        exception: Exception, an exception will be raised when timed out if needed.
+        interval_sec: float, interval time between calls of the condition function
+            in seconds.
 
-  Returns:
-    True if the function returns the expected value else False.
-  """
-  start_time = time.time()
-  end_time = start_time + timeout_sec
-  while time.time() < end_time:
-    if condition_func(*func_args) == expected_value:
-      return True
-    time.sleep(interval_sec)
-  args_string = ', '.join(list(map(str, func_args)))
-  log.warning('Timed out after %.1fs waiting for "%s(%s)" to be "%s".',
-              timeout_sec, condition_func.__name__, args_string, expected_value)
-  if exception:
-    raise exception
-  return False
+    Returns:
+        True if the function returns the expected value else False.
+    """
+    start_time = time.time()
+    end_time = start_time + timeout_sec
+    while time.time() < end_time:
+        if condition_func(*func_args) == expected_value:
+            return True
+        time.sleep(interval_sec)
+    args_string = ', '.join(list(map(str, func_args)))
+    log.warning('Timed out after %.1fs waiting for "%s(%s)" to be "%s".', timeout_sec, condition_func.__name__,
+                args_string, expected_value)
+    if exception:
+        raise exception
+    return False
 
 
 def write_read_verify_data_sl4a(client_ad, server_ad, msg, binary=False):
-  """Verify that the client wrote data to the server Android device correctly.
+    """Verify that the client wrote data to the server Android device correctly.
 
-  Args:
-      client_ad: the Android device to perform the write.
-      server_ad: the Android device to read the data written.
-      msg: the message to write.
-      binary: if the msg arg is binary or not.
+    Args:
+        client_ad: the Android device to perform the write.
+        server_ad: the Android device to read the data written.
+        msg: the message to write.
+        binary: if the msg arg is binary or not.
 
-  Returns:
-      True if the data written matches the data read, false if not.
-  """
-  client_ad.log.info('Write message %s.', msg)
-  if binary:
-    client_ad.sl4a.bluetoothSocketConnWriteBinary(msg)
-  else:
-    client_ad.sl4a.bluetoothSocketConnWrite(msg)
-  server_ad.log.info('Read message %s.', msg)
-  if binary:
-    read_msg = server_ad.sl4a.bluetoothSocketConnReadBinary().rstrip('\r\n')
-  else:
-    read_msg = server_ad.sl4a.bluetoothSocketConnRead()
-  log.info('Verify message.')
-  if msg != read_msg:
-    log.error('Mismatch! Read: %s, Expected: %s', read_msg, msg)
-    return False
-  log.info('Matched! Read: %s, Expected: %s', read_msg, msg)
-  return True
+    Returns:
+        True if the data written matches the data read, false if not.
+    """
+    client_ad.log.info('Write message %s.', msg)
+    if binary:
+        client_ad.sl4a.bluetoothSocketConnWriteBinary(msg)
+    else:
+        client_ad.sl4a.bluetoothSocketConnWrite(msg)
+    server_ad.log.info('Read message %s.', msg)
+    if binary:
+        read_msg = server_ad.sl4a.bluetoothSocketConnReadBinary().rstrip('\r\n')
+    else:
+        read_msg = server_ad.sl4a.bluetoothSocketConnRead()
+    log.info('Verify message.')
+    if msg != read_msg:
+        log.error('Mismatch! Read: %s, Expected: %s', read_msg, msg)
+        return False
+    log.info('Matched! Read: %s, Expected: %s', read_msg, msg)
+    return True
 
 
 def write_record_file(file_name, audio_params, frames):
-  """Writes the recorded audio into the file.
+    """Writes the recorded audio into the file.
 
-  Args:
-      file_name: The file name for writing the recorded audio.
-      audio_params: A dict with audio configuration.
-      frames: Recorded audio frames.
-  """
-  log.debug('writing frame to %s', file_name)
-  wf = wave.open(file_name, 'wb')
-  wf.setnchannels(audio_params['channel'])
-  wf.setsampwidth(audio_params.get('sample_width', 1))
-  wf.setframerate(audio_params['sample_rate'])
-  wf.writeframes(frames)
-  wf.close()
+    Args:
+        file_name: The file name for writing the recorded audio.
+        audio_params: A dict with audio configuration.
+        frames: Recorded audio frames.
+    """
+    log.debug('writing frame to %s', file_name)
+    wf = wave.open(file_name, 'wb')
+    wf.setnchannels(audio_params['channel'])
+    wf.setsampwidth(audio_params.get('sample_width', 1))
+    wf.setframerate(audio_params['sample_rate'])
+    wf.writeframes(frames)
+    wf.close()
+
+
+def get_mac_address_of_generic_advertisement(scan_device, adv_device):
+    """Start generic advertisement and get it's mac address by LE scanning.
+
+    Args:
+        scan_ad: The Android device to use as the scanner.
+        adv_device: The Android device to use as the advertiser.
+
+    Returns:
+        mac_address: The mac address of the advertisement.
+        advertise_callback: The advertise callback id of the active
+            advertisement.
+    """
+    adv_device.sl4a.bleSetAdvertiseDataIncludeDeviceName(True)
+    adv_device.sl4a.bleSetAdvertiseSettingsAdvertiseMode(ble_advertise_settings_modes['low_latency'])
+    adv_device.sl4a.bleSetAdvertiseSettingsIsConnectable(True)
+    adv_device.sl4a.bleSetAdvertiseSettingsTxPowerLevel(ble_advertise_settings_tx_powers['high'])
+    advertise_callback, advertise_data, advertise_settings = (generate_ble_advertise_objects(adv_device.sl4a))
+    adv_device.sl4a.bleStartBleAdvertising(advertise_callback, advertise_data, advertise_settings)
+    try:
+        adv_device.ed.pop_event(adv_succ.format(advertise_callback), bt_default_timeout)
+    except Empty as err:
+        raise BtTestUtilsError("Advertiser did not start successfully {}".format(err))
+    filter_list = scan_device.sl4a.bleGenFilterList()
+    scan_settings = scan_device.sl4a.bleBuildScanSetting()
+    scan_callback = scan_device.sl4a.bleGenScanCallback()
+    scan_device.sl4a.bleSetScanFilterDeviceName(adv_device.sl4a.bluetoothGetLocalName())
+    scan_device.sl4a.bleBuildScanFilter(filter_list)
+    scan_device.sl4a.bleStartBleScan(filter_list, scan_settings, scan_callback)
+    try:
+        event = scan_device.sl4a.ed.pop_event("BleScan{}onScanResults".format(scan_callback), bt_default_timeout)
+    except Empty as err:
+        raise BtTestUtilsError("Scanner did not find advertisement {}".format(err))
+    mac_address = event['data']['Result']['deviceInfo']['address']
+    return mac_address, advertise_callback, scan_callback
+
+
+def clear_bonded_devices(ad: AndroidDevice):
+    """Clear bonded devices from the input Android device.
+
+    Args:
+        ad: the Android device performing the connection.
+    Returns:
+        True if clearing bonded devices was successful, false if unsuccessful.
+    """
+    bonded_device_list = ad.sl4a.bluetoothGetBondedDevices()
+    while bonded_device_list:
+        device_address = bonded_device_list[0]['address']
+        if not ad.sl4a.bluetoothUnbond(device_address):
+            ad.log.error("Failed to unbond {} from {}".format(device_address, ad.serial))
+            return False
+        ad.log.info("Successfully unbonded {} from {}".format(device_address, ad.serial))
+        #TODO: wait for BOND_STATE_CHANGED intent instead of waiting
+        time.sleep(1)
+
+        # If device was first connected using LE transport, after bonding it is
+        # accessible through it's LE address, and through it classic address.
+        # Unbonding it will unbond two devices representing different
+        # "addresses". Attempt to unbond such already unbonded devices will
+        # result in bluetoothUnbond returning false.
+        bonded_device_list = ad.sl4a.bluetoothGetBondedDevices()
+    return True
diff --git a/system/blueberry/utils/mobly_sl4a_utils.py b/system/blueberry/utils/mobly_sl4a_utils.py
new file mode 100644
index 0000000..0d06d16
--- /dev/null
+++ b/system/blueberry/utils/mobly_sl4a_utils.py
@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+#
+# Copyright (C) 2016 The Android Open Source Project
+#
+# 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.
+
+import logging
+
+import mobly.controllers.android_device_lib.sl4a_client as sl4a_client
+from mobly.controllers.android_device import AndroidDevice
+from mobly.controllers.android_device_lib.adb import AdbError
+from mobly.controllers.android_device_lib.jsonrpc_client_base import AppRestoreConnectionError
+from mobly.controllers.android_device_lib.services.sl4a_service import Sl4aService
+
+
+class FakeFuture:
+    """
+    A fake Future object to override default mobly behavior
+    """
+
+    def set_result(self, result):
+        logging.debug("Setting fake result {}".format(result))
+
+
+def setup_sl4a(device: AndroidDevice, server_port: int, forwarded_port: int):
+    """
+    A method that setups up SL4A instance on mobly
+    :param device: an AndroidDevice instance
+    :param server_port: Preferred server port used by SL4A on Android device
+    :param forwarded_port: Preferred server port number forwarded from Android to
+                           the host PC via adb for SL4A connections
+    :return: None
+    """
+    sl4a_client._DEVICE_SIDE_PORT = server_port
+    sl4a_client._APP_START_WAIT_TIME = 0.5
+    if device.sl4a is not None:
+        device.log.error("SL4A is not none when registering")
+    device.services.register('sl4a', Sl4aService, start_service=False)
+    # Start the SL4A service and event dispatcher
+    try:
+        device.sl4a.start()
+    except AppRestoreConnectionError as exp:
+        device.log.debug("AppRestoreConnectionError {}".format(exp))
+    # Pause the dispatcher, but do not stop the service
+    try:
+        device.sl4a.pause()
+    except AdbError as exp:
+        device.log.debug("Failed to pause() {}".format(exp))
+    sl4a_client._APP_START_WAIT_TIME = 2 * 60
+    # Restart the service with a new host port
+    device.sl4a.restore_app_connection(port=forwarded_port)
+
+
+def teardown_sl4a(device: AndroidDevice):
+    """
+    A method to tear down SL4A interface on mobly
+    :param device: an AndroidDevice instance that already contains SL4a
+    :return: None
+    """
+    if device.sl4a.is_alive:
+        # Both self.dut.sl4a and self.dut.sl4a.ed._sl4a are Sl4aClient instances
+        # If we do not set host_port to None here, host_poart will be removed twice from Android
+        # The 2nd removal will trigger and exception that spam the test result
+        # TODO: Resolve this issue in mobly
+        device.log.info("Clearing host_port to prevent mobly crash {}".format(device.sl4a._sl4a_client.host_port))
+        device.sl4a._sl4a_client.host_port = None
+        # Moreover concurrent.Future.set_result() should never be called from thread that is
+        # waiting for the future. However, mobly calls it and cause InvalidStateError when it
+        # tries to do that after the thread pool has stopped, overriding it here
+        # TODO: Resolve this issue in mobly
+        device.sl4a.ed.poller = FakeFuture()
+    try:
+        # Guarded by is_alive internally
+        device.sl4a.stop()
+    except AdbError as exp:
+        device.log.warning("Failed top stop()".format(exp))
diff --git a/system/bta/Android.bp b/system/bta/Android.bp
index 902c0a6..26e1513 100644
--- a/system/bta/Android.bp
+++ b/system/bta/Android.bp
@@ -413,6 +413,7 @@
         "packages/modules/Bluetooth/system",
         "packages/modules/Bluetooth/system/bta/include",
         "packages/modules/Bluetooth/system/bta/test/common",
+        "packages/modules/Bluetooth/system/gd",
         "packages/modules/Bluetooth/system/stack/include",
     ],
     srcs : [
@@ -605,6 +606,8 @@
         "clang_coverage_bin",
     ],
     host_supported: true,
+    // TODO(b/231993739): Reenable isolated:true by deleting the explicit disable below
+    isolated: false,
     include_dirs: [
         "packages/modules/Bluetooth/system",
         "packages/modules/Bluetooth/system/bta/include",
@@ -691,6 +694,8 @@
         "clang_coverage_bin",
     ],
     host_supported: true,
+    // TODO(b/231993739): Reenable isolated:true by deleting the explicit disable below
+    isolated: false,
     include_dirs: [
         "packages/modules/Bluetooth/system",
         "packages/modules/Bluetooth/system/bta/include",
@@ -805,6 +810,8 @@
         "clang_coverage_bin",
     ],
     host_supported: true,
+    // TODO(b/231993739): Reenable isolated:true by deleting the explicit disable below
+    isolated: false,
     include_dirs: [
         "packages/modules/Bluetooth/system",
         "packages/modules/Bluetooth/system/bta/include",
@@ -847,4 +854,4 @@
             undefined : true
         },
     },
-}
\ No newline at end of file
+}
diff --git a/system/bta/BUILD.gn b/system/bta/BUILD.gn
index db8db54..1e2d6cd 100644
--- a/system/bta/BUILD.gn
+++ b/system/bta/BUILD.gn
@@ -107,6 +107,7 @@
     "//bt/system/",
     "//bt/system/linux_include",
     "//bt/system/bta",
+    "//bt/system/gd",
     "//bt/system/internal_include",
     "//bt/system/internal_include",
     "//bt/system/stack/include",
diff --git a/system/bta/hearing_aid/hearing_aid.cc b/system/bta/hearing_aid/hearing_aid.cc
index 105221d..987aaec 100644
--- a/system/bta/hearing_aid/hearing_aid.cc
+++ b/system/bta/hearing_aid/hearing_aid.cc
@@ -402,15 +402,6 @@
       hearingDevice->connection_update_status = AWAITING;
     }
 
-    if (controller_get_interface()->supports_ble_2m_phy()) {
-      LOG(INFO) << address << " set preferred PHY to 2M";
-      BTM_BleSetPhy(address, PHY_LE_2M, PHY_LE_2M, 0);
-    }
-
-    // Set data length
-    // TODO(jpawlowski: for 16khz only 87 is required, optimize
-    BTM_SetBleDataLength(address, 167);
-
     if (BTM_SecIsSecurityPending(address)) {
       /* if security collision happened, wait for encryption done
        * (BTA_GATTC_ENC_CMPL_CB_EVT) */
@@ -510,6 +501,12 @@
       hearingDevice->connection_update_status = NONE;
     }
 
+    if (!hearingDevice->accepting_audio &&
+        hearingDevice->connection_update_status == COMPLETED &&
+        hearingDevice->gap_opened) {
+      OnDeviceReady(hearingDevice->address);
+    }
+
     for (auto& device : hearingDevices.devices) {
       if (device.conn_id && (device.connection_update_status == AWAITING)) {
         device.connection_update_status = STARTED;
@@ -578,6 +575,24 @@
     }
   }
 
+  void OnPhyUpdateEvent(uint16_t conn_id, uint8_t tx_phys, uint8_t rx_phys,
+                        tGATT_STATUS status) {
+    HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id);
+    if (!hearingDevice) {
+      DVLOG(2) << "Skipping unknown device, conn_id=" << loghex(conn_id);
+      return;
+    }
+    if (status == GATT_SUCCESS && tx_phys == PHY_LE_2M &&
+        rx_phys == PHY_LE_2M) {
+      LOG(INFO) << hearingDevice->address << " phy update to 2M successful";
+      return;
+    }
+    LOG(INFO) << hearingDevice->address
+              << " phy update to 2M fail, try again. status: " << status
+              << ", tx_phys: " << tx_phys << ", rx_phys: " << rx_phys;
+    BTM_BleSetPhy(hearingDevice->address, PHY_LE_2M, PHY_LE_2M, 0);
+  }
+
   void OnServiceChangeEvent(const RawAddress& address) {
     HearingDevice* hearingDevice = hearingDevices.FindByAddress(address);
     if (!hearingDevice) {
@@ -875,6 +890,15 @@
   void ConnectSocket(HearingDevice* hearingDevice, uint16_t psm) {
     tL2CAP_CFG_INFO cfg_info = tL2CAP_CFG_INFO{.mtu = 512};
 
+    if (controller_get_interface()->supports_ble_2m_phy()) {
+      LOG(INFO) << hearingDevice->address << " set preferred PHY to 2M";
+      BTM_BleSetPhy(hearingDevice->address, PHY_LE_2M, PHY_LE_2M, 0);
+    }
+
+    // Set data length
+    // TODO(jpawlowski: for 16khz only 87 is required, optimize
+    BTM_SetBleDataLength(hearingDevice->address, 167);
+
     SendEnableServiceChangedInd(hearingDevice);
 
     uint8_t service_id = hearingDevice->isLeft()
@@ -915,8 +939,8 @@
       instance->OnPsmRead(conn_id, status, handle, len, value, data);
   }
 
-  /* CoC Socket is ready */
-  void OnGapConnection(const RawAddress& address) {
+  /* CoC Socket, BLE connection parameter are ready */
+  void OnDeviceReady(const RawAddress& address) {
     HearingDevice* hearingDevice = hearingDevices.FindByAddress(address);
     if (!hearingDevice) {
       LOG(INFO) << "Device not connected to profile" << address;
@@ -1398,7 +1422,16 @@
 
         LOG(INFO) << "GAP_EVT_CONN_OPENED " << address << ", tx_mtu=" << tx_mtu
                   << ", init_credit=" << init_credit;
-        OnGapConnection(address);
+
+        HearingDevice* hearingDevice = hearingDevices.FindByAddress(address);
+        if (!hearingDevice) {
+          LOG(INFO) << "Skipping unknown device" << address;
+          return;
+        }
+        hearingDevice->gap_opened = true;
+        if (hearingDevice->connection_update_status == COMPLETED) {
+          OnDeviceReady(address);
+        }
         break;
       }
 
@@ -1417,6 +1450,7 @@
           hearingDevice->accepting_audio = false;
           hearingDevice->playback_started = false;
           hearingDevice->command_acked = false;
+          hearingDevice->gap_opened = false;
         }
         break;
       case GAP_EVT_CONN_DATA_AVAIL: {
@@ -1631,6 +1665,7 @@
       }
     }
     hearingDevice->connection_update_status = NONE;
+    hearingDevice->gap_opened = false;
 
     if (hearingDevice->conn_id) {
       BtaGattQueue::Clean(hearingDevice->conn_id);
@@ -1850,6 +1885,12 @@
       if (!instance) return;
       instance->OnServiceDiscDoneEvent(p_data->service_changed.remote_bda);
       break;
+    case BTA_GATTC_PHY_UPDATE_EVT: {
+      if (!instance) return;
+      tBTA_GATTC_PHY_UPDATE& p = p_data->phy_update;
+      instance->OnPhyUpdateEvent(p.conn_id, p.tx_phy, p.rx_phy, p.status);
+      break;
+    }
 
     default:
       break;
diff --git a/system/bta/include/bta_hearing_aid_api.h b/system/bta/include/bta_hearing_aid_api.h
index d1b8cb3..90803c5 100644
--- a/system/bta/include/bta_hearing_aid_api.h
+++ b/system/bta/include/bta_hearing_aid_api.h
@@ -158,6 +158,8 @@
   int read_rssi_count;
   int num_intervals_since_last_rssi_read;
 
+  bool gap_opened;
+
   HearingDevice(const RawAddress& address, uint8_t capabilities,
                 uint16_t codecs, uint16_t audio_control_point_handle,
                 uint16_t audio_status_handle, uint16_t audio_status_ccc_handle,
@@ -185,7 +187,8 @@
         codecs(codecs),
         playback_started(false),
         command_acked(false),
-        read_rssi_count(0) {}
+        read_rssi_count(0),
+        gap_opened(false) {}
 
   HearingDevice(const RawAddress& address, bool first_connection)
       : address(address),
@@ -207,7 +210,8 @@
         codecs(0),
         playback_started(false),
         command_acked(false),
-        read_rssi_count(0) {}
+        read_rssi_count(0),
+        gap_opened(false) {}
 
   HearingDevice() : HearingDevice(RawAddress::kEmpty, false) {}
 
diff --git a/system/bta/include/bta_vc_api.h b/system/bta/include/bta_vc_api.h
index b8913eb..3b2db1b 100644
--- a/system/bta/include/bta_vc_api.h
+++ b/system/bta/include/bta_vc_api.h
@@ -39,6 +39,9 @@
   virtual void Disconnect(const RawAddress& address) = 0;
   virtual void SetVolume(std::variant<RawAddress, int> addr_or_group_id,
                          uint8_t volume) = 0;
+  virtual void Mute(std::variant<RawAddress, int> addr_or_group_id) = 0;
+  virtual void UnMute(std::variant<RawAddress, int> addr_or_group_id) = 0;
+
   /* Volume Offset Control Service (VOCS) */
   virtual void SetExtAudioOutVolumeOffset(const RawAddress& address,
                                           uint8_t ext_output_id,
diff --git a/system/bta/le_audio/audio_set_configurations.json b/system/bta/le_audio/audio_set_configurations.json
index ef744c6..d0f9cbd 100644
--- a/system/bta/le_audio/audio_set_configurations.json
+++ b/system/bta/le_audio/audio_set_configurations.json
@@ -233,6 +233,16 @@
             "qos_config_name": ["QoS_Config_16_1_1"]
         },
         {
+            "name": "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_32_2_Server_Preferred",
+            "codec_config_name": "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_32_2",
+            "qos_config_name": ["QoS_Config_Server_Preferred"]
+        },
+        {
+            "name": "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_32_2_1",
+            "codec_config_name": "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_32_2",
+            "qos_config_name": ["QoS_Config_32_2_1"]
+        },
+        {
             "name": "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_2_Server_Preferred",
             "codec_config_name": "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_2",
             "qos_config_name": ["QoS_Config_Server_Preferred"]
@@ -413,6 +423,66 @@
             "qos_config_name": ["QoS_Config_24_2_2"]
         },
         {
+            "name": "DualDev_OneChanStereoSnk_OneChanStereoSrc_32_2_Server_Preferred",
+            "codec_config_name": "DualDev_OneChanStereoSnk_OneChanStereoSrc_32_2",
+            "qos_config_name": ["QoS_Config_Server_Preferred"]
+        },
+        {
+            "name": "DualDev_OneChanStereoSnk_OneChanStereoSrc_32_2_1",
+            "codec_config_name": "DualDev_OneChanStereoSnk_OneChanStereoSrc_32_2",
+            "qos_config_name": ["QoS_Config_32_2_1"]
+        },
+        {
+            "name": "DualDev_OneChanStereoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+            "codec_config_name": "DualDev_OneChanStereoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_Server_Preferred"]
+        },
+        {
+            "name": "DualDev_OneChanStereoSnk_OneChanMonoSrc_32_2_1",
+            "codec_config_name": "DualDev_OneChanStereoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_32_2_1"]
+        },
+        {
+            "name": "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+            "codec_config_name": "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_Server_Preferred"]
+        },
+        {
+            "name": "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_32_2_1",
+            "codec_config_name": "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_32_2_1"]
+        },
+        {
+            "name": "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+            "codec_config_name": "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_Server_Preferred"]
+        },
+        {
+            "name": "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_32_2_1",
+            "codec_config_name": "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_32_2_1"]
+        },
+        {
+            "name": "SingleDev_OneChanStereoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+            "codec_config_name": "SingleDev_OneChanStereoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_Server_Preferred"]
+        },
+        {
+            "name": "SingleDev_OneChanStereoSnk_OneChanMonoSrc_32_2_1",
+            "codec_config_name": "SingleDev_OneChanStereoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_32_2_1"]
+        },
+        {
+            "name": "SingleDev_OneChanMonoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+            "codec_config_name": "SingleDev_OneChanMonoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_Server_Preferred"]
+        },
+        {
+            "name": "SingleDev_OneChanMonoSnk_OneChanMonoSrc_32_2_1",
+            "codec_config_name": "SingleDev_OneChanMonoSnk_OneChanMonoSrc_32_2",
+            "qos_config_name": ["QoS_Config_32_2_1"]
+        },
+        {
             "name": "DualDev_OneChanStereoSnk_48_4_Server_Preferred",
             "codec_config_name": "DualDev_OneChanStereoSnk_48_4",
             "qos_config_name": ["QoS_Config_Server_Preferred"]
@@ -1845,6 +1915,135 @@
             ]
         },
         {
+            "name": "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_32_2",
+            "subconfigurations": [
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SINK",
+                    "configuration_strategy": "STEREO_ONE_CIS_PER_DEVICE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    3,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                },
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SOURCE",
+                    "configuration_strategy": "STEREO_ONE_CIS_PER_DEVICE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    3,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                }
+            ]
+        },
+        {
             "name": "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_2",
             "subconfigurations": [
                 {
@@ -3995,6 +4194,771 @@
                     ]
                 }
             ]
+        },
+        {
+            "name": "DualDev_OneChanStereoSnk_OneChanStereoSrc_32_2",
+            "subconfigurations": [
+                {
+                    "device_cnt": 2,
+                    "ase_cnt": 2,
+                    "direction": "SINK",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                },
+                {
+                    "device_cnt": 2,
+                    "ase_cnt": 2,
+                    "direction": "SOURCE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                }
+            ]
+        },
+        {
+            "name": "DualDev_OneChanStereoSnk_OneChanMonoSrc_32_2",
+            "subconfigurations": [
+                {
+                    "device_cnt": 2,
+                    "ase_cnt": 2,
+                    "direction": "SINK",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                },
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SOURCE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                }
+            ]
+        },
+        {
+            "name": "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_32_2",
+            "subconfigurations": [
+                {
+                    "device_cnt": 2,
+                    "ase_cnt": 4,
+                    "direction": "SINK",
+                    "configuration_strategy": "STEREO_TWO_CISES_PER_DEVICE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                },
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SOURCE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                }
+            ]
+        },
+        {
+            "name": "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_32_2",
+            "subconfigurations": [
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SINK",
+                    "configuration_strategy": "STEREO_ONE_CIS_PER_DEVICE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    3,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                },
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SOURCE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                }
+            ]
+        },
+        {
+            "name": "SingleDev_OneChanStereoSnk_OneChanMonoSrc_32_2",
+            "subconfigurations": [
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 2,
+                    "direction": "SINK",
+                    "configuration_strategy": "STEREO_TWO_CISES_PER_DEVICE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                },
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SOURCE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                }
+            ]
+        },
+        {
+            "name": "SingleDev_OneChanMonoSnk_OneChanMonoSrc_32_2",
+            "subconfigurations": [
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SINK",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                },
+                {
+                    "device_cnt": 1,
+                    "ase_cnt": 1,
+                    "direction": "SOURCE",
+                    "codec_id": {
+                        "coding_format": 6,
+                        "vendor_company_id": 0,
+                        "vendor_codec_id": 0
+                    },
+                    "codec_configuration": [
+                        {
+                            "name": "sampling_frequency",
+                            "type": 1,
+                            "compound_value": {
+                                "value": [
+                                    6
+                                ]
+                            }
+                        },
+                        {
+                            "name": "frame_duration",
+                            "type": 2,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        },
+                        {
+                            "name": "audio_channel_allocation",
+                            "type": 3,
+                            "compound_value": {
+                                "value": [
+                                    1,
+                                    0,
+                                    0,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "octets_per_codec_frame",
+                            "type": 4,
+                            "compound_value": {
+                                "value": [
+                                    80,
+                                    0
+                                ]
+                            }
+                        },
+                        {
+                            "name": "codec_frame_blocks_per_sdu",
+                            "type": 5,
+                            "compound_value": {
+                                "value": [
+                                    1
+                                ]
+                            }
+                        }
+                    ]
+                }
+            ]
         }
     ],
     "qos_configurations": [
@@ -4024,6 +4988,11 @@
             "max_transport_latency": 95
         },
         {
+            "name": "QoS_Config_32_2_1",
+            "retransmission_number": 2,
+            "max_transport_latency": 10
+        },
+        {
             "name": "QoS_Config_48_4_1",
             "retransmission_number": 5,
             "max_transport_latency": 20
diff --git a/system/bta/le_audio/audio_set_scenarios.json b/system/bta/le_audio/audio_set_scenarios.json
index a79f47b..ee946b4 100644
--- a/system/bta/le_audio/audio_set_scenarios.json
+++ b/system/bta/le_audio/audio_set_scenarios.json
@@ -29,34 +29,48 @@
         {
             "name": "Conversational",
             "configurations": [
+                "DualDev_OneChanStereoSnk_OneChanStereoSrc_32_2_Server_Preferred",
+                "DualDev_OneChanStereoSnk_OneChanStereoSrc_32_2_1",
                 "DualDev_OneChanStereoSnk_OneChanStereoSrc_16_2_Server_Preferred",
                 "DualDev_OneChanStereoSnk_OneChanStereoSrc_16_2_1",
                 "DualDev_OneChanStereoSnk_OneChanStereoSrc_16_2_2",
                 "DualDev_OneChanStereoSnk_OneChanStereoSrc_16_1_Server_Preferred",
                 "DualDev_OneChanStereoSnk_OneChanStereoSrc_16_1_1",
                 "DualDev_OneChanStereoSnk_OneChanStereoSrc_16_1_2",
+                "DualDev_OneChanStereoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+                "DualDev_OneChanStereoSnk_OneChanMonoSrc_32_2_1",
                 "DualDev_OneChanStereoSnk_OneChanMonoSrc_16_2_Server_Preferred",
                 "DualDev_OneChanStereoSnk_OneChanMonoSrc_16_2_1",
                 "DualDev_OneChanStereoSnk_OneChanMonoSrc_16_1_Server_Preferred",
                 "DualDev_OneChanStereoSnk_OneChanMonoSrc_16_1_1",
+                "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+                "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_32_2_1",
                 "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_16_2_Server_Preferred",
                 "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_16_2_1",
                 "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_16_1_Server_Preferred",
                 "DualDev_OneChanDoubleStereoSnk_OneChanMonoSrc_16_1_1",
+                "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_32_2_Server_Preferred",
+                "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_32_2_1",
                 "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_2_Server_Preferred",
                 "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_2_1",
                 "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_2_2",
                 "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_1_Server_Preferred",
                 "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_1_1",
                 "SingleDev_TwoChanStereoSnk_TwoChanStereoSrc_16_1_2",
+                "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+                "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_32_2_1",
                 "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_16_2_Server_Preferred",
                 "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_16_2_1",
                 "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_16_1_Server_Preferred",
                 "SingleDev_TwoChanStereoSnk_OneChanMonoSrc_16_1_1",
+                "SingleDev_OneChanStereoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+                "SingleDev_OneChanStereoSnk_OneChanMonoSrc_32_2_1",
                 "SingleDev_OneChanStereoSnk_OneChanMonoSrc_16_2_Server_Preferred",
                 "SingleDev_OneChanStereoSnk_OneChanMonoSrc_16_2_1",
                 "SingleDev_OneChanStereoSnk_OneChanMonoSrc_16_1_Server_Preferred",
                 "SingleDev_OneChanStereoSnk_OneChanMonoSrc_16_1_1",
+                "SingleDev_OneChanMonoSnk_OneChanMonoSrc_32_2_Server_Preferred",
+                "SingleDev_OneChanMonoSnk_OneChanMonoSrc_32_2_1",
                 "SingleDev_OneChanMonoSnk_OneChanMonoSrc_16_2_Server_Preferred",
                 "SingleDev_OneChanMonoSnk_OneChanMonoSrc_16_2_1",
                 "SingleDev_OneChanMonoSnk_OneChanMonoSrc_16_1_Server_Preferred",
diff --git a/system/bta/le_audio/client.cc b/system/bta/le_audio/client.cc
index 7547a97..7417115 100644
--- a/system/bta/le_audio/client.cc
+++ b/system/bta/le_audio/client.cc
@@ -116,6 +116,23 @@
 namespace {
 void le_audio_gattc_callback(tBTA_GATTC_EVT event, tBTA_GATTC* p_data);
 
+inline uint8_t bits_to_bytes_per_sample(uint8_t bits_per_sample) {
+  // 24 bit audio stream is sent as unpacked, each sample takes 4 bytes.
+  if (bits_per_sample == 24) return 4;
+
+  return bits_per_sample / 8;
+}
+
+inline lc3_pcm_format bits_to_lc3_bits(uint8_t bits_per_sample) {
+  if (bits_per_sample == 16) return LC3_PCM_FORMAT_S16;
+
+  if (bits_per_sample == 24) return LC3_PCM_FORMAT_S24;
+
+  LOG_ALWAYS_FATAL("Encoder/decoder don't know how to handle %d",
+                   bits_per_sample);
+  return LC3_PCM_FORMAT_S16;
+}
+
 class LeAudioClientImpl;
 LeAudioClientImpl* instance;
 LeAudioClientAudioSinkReceiver* audioSinkReceiver;
@@ -684,6 +701,52 @@
     // TODO Implement
   }
 
+  void StartAudioSession(LeAudioDeviceGroup* group,
+                          LeAudioCodecConfiguration* source_config,
+                          LeAudioCodecConfiguration* sink_config) {
+    /* This function is called when group is not yet set to active.
+     * This is why we don't have to check if session is started already.
+     * Just check if it is acquired.
+     */
+    ASSERT_LOG(active_group_id_ == bluetooth::groups::kGroupUnknown,
+               "Active group is not set.");
+    ASSERT_LOG(audio_source_instance_, "Source session not acquired");
+    ASSERT_LOG(audio_sink_instance_, "Sink session not acquired");
+
+    /* We assume that peer device always use same frame duration */
+    uint32_t frame_duration_us = 0;
+    if (!source_config->IsInvalid()) {
+      frame_duration_us = source_config->data_interval_us;
+    } else if (!sink_config->IsInvalid()) {
+      frame_duration_us = sink_config->data_interval_us;
+    } else {
+      ASSERT_LOG(true, "Both configs are invalid");
+    }
+
+    audio_framework_source_config.data_interval_us = frame_duration_us;
+    leAudioClientAudioSource->Start(audio_framework_source_config,
+                                    audioSinkReceiver);
+
+    /* We use same frame duration for sink/source */
+    audio_framework_sink_config.data_interval_us = frame_duration_us;
+
+    /* If group supports more than 16kHz for the microphone in converstional
+     * case let's use that also for Audio Framework.
+     */
+    std::optional<LeAudioCodecConfiguration> sink_configuration =
+        group->GetCodecConfigurationByDirection(
+            LeAudioContextType::CONVERSATIONAL,
+            le_audio::types::kLeAudioDirectionSource);
+    if (sink_configuration &&
+        sink_configuration->sample_rate >
+            bluetooth::audio::le_audio::kSampleRate16000) {
+      audio_framework_sink_config.sample_rate = sink_configuration->sample_rate;
+    }
+
+    leAudioClientAudioSink->Start(audio_framework_sink_config,
+                                  audioSourceReceiver);
+  }
+
   void GroupSetActive(const int group_id) override {
     DLOG(INFO) << __func__ << " group_id: " << group_id;
 
@@ -752,16 +815,8 @@
 
     if (active_group_id_ == bluetooth::groups::kGroupUnknown) {
       /* Expose audio sessions if there was no previous active group */
-      audio_framework_source_config.data_interval_us =
-          current_source_codec_config.data_interval_us;
-      leAudioClientAudioSource->Start(audio_framework_source_config,
-                                      audioSinkReceiver);
-
-      audio_framework_sink_config.data_interval_us =
-          current_source_codec_config.data_interval_us;
-
-      leAudioClientAudioSink->Start(audio_framework_sink_config,
-                                    audioSourceReceiver);
+      StartAudioSession(group, &current_source_codec_config,
+                         &current_sink_codec_config);
     } else {
       /* In case there was an active group. Stop the stream */
       GroupStop(active_group_id_);
@@ -1885,27 +1940,36 @@
     return true;
   }
 
-  void get_mono_stream(const std::vector<uint8_t>& data,
-                       std::vector<int16_t>& chan_mono, int pitch = 1) {
-    uint16_t num_of_frames_per_ch;
+  // mix stero signal into mono
+  std::vector<uint8_t> mono_blend(const std::vector<uint8_t>& buf,
+                                  int bytes_per_sample, size_t frames) {
+    std::vector<uint8_t> mono_out;
+    mono_out.resize(frames * bytes_per_sample);
 
-    int dt_us = current_source_codec_config.data_interval_us;
-    int af_hz = audio_framework_source_config.sample_rate;
-    num_of_frames_per_ch = lc3_frame_samples(dt_us, af_hz);
-
-    chan_mono.reserve(num_of_frames_per_ch);
-    for (int i = 0; i < pitch * num_of_frames_per_ch; i += pitch) {
-      const uint8_t* sample = data.data() + i * 4;
-
-      int16_t left = (int16_t)((*(sample + 1) << 8) + *sample) >> 1;
-
-      sample += 2;
-      int16_t right = (int16_t)((*(sample + 1) << 8) + *sample) >> 1;
-
-      uint16_t mono_data = (int16_t)(((uint32_t)left + (uint32_t)right) >> 1);
-
-      chan_mono.push_back(mono_data);
+    if (bytes_per_sample == 2) {
+      int16_t* out = (int16_t*)mono_out.data();
+      for (size_t i = 0; i < frames; ++i) {
+        const int16_t* in = (int16_t*)(buf.data());
+        int accum = 0;
+        accum += *in++;
+        accum += *in++;
+        accum /= 2;  // round to 0
+        *out++ = accum;
+      }
+    } else if (bytes_per_sample == 4) {
+      int32_t* out = (int32_t*)mono_out.data();
+      for (size_t i = 0; i < frames; ++i) {
+        const int32_t* in = (int32_t*)(buf.data());
+        int accum = 0;
+        accum += *in++;
+        accum += *in++;
+        accum /= 2;  // round to 0
+        *out++ = accum;
+      }
+    } else {
+      LOG_ERROR("Don't know how to mono blend that %d!", bytes_per_sample);
     }
+    return mono_out;
   }
 
   void PrepareAndSendToTwoDevices(
@@ -1920,6 +1984,11 @@
     int af_hz = audio_framework_source_config.sample_rate;
     number_of_required_samples_per_channel = lc3_frame_samples(dt_us, af_hz);
 
+    lc3_pcm_format bits_per_sample =
+        bits_to_lc3_bits(audio_framework_source_config.bits_per_sample);
+    uint8_t bytes_per_sample =
+        bits_to_bytes_per_sample(audio_framework_source_config.bits_per_sample);
+
     for (auto [cis_handle, audio_location] : stream_conf->sink_streams) {
       if (audio_location & le_audio::codec_spec_conf::kLeAudioLocationAnyLeft)
         left_cis_handle = cis_handle;
@@ -1927,11 +1996,12 @@
         right_cis_handle = cis_handle;
     }
 
-    if (data.size() < 2 /* bytes per sample */ * 2 /* channels */ *
+    if (data.size() < bytes_per_sample * 2 /* channels */ *
                           number_of_required_samples_per_channel) {
       LOG(ERROR) << __func__ << " Missing samples. Data size: " << +data.size()
                  << " expected: "
-                 << 2 * 2 * number_of_required_samples_per_channel;
+                 << bytes_per_sample * 2 *
+                        number_of_required_samples_per_channel;
       return;
     }
 
@@ -1941,26 +2011,22 @@
     bool mono = (left_cis_handle == 0) || (right_cis_handle == 0);
 
     if (!mono) {
-      lc3_encode(lc3_encoder_left, LC3_PCM_FORMAT_S16,
-                 (const int16_t*)data.data(), 2, chan_left_enc.size(),
-                 chan_left_enc.data());
-      lc3_encode(lc3_encoder_right, LC3_PCM_FORMAT_S16,
-                 ((const int16_t*)data.data()) + 1, 2, chan_right_enc.size(),
+      lc3_encode(lc3_encoder_left, bits_per_sample, data.data(), 2,
+                 chan_left_enc.size(), chan_left_enc.data());
+      lc3_encode(lc3_encoder_right, bits_per_sample,
+                 data.data() + bytes_per_sample, 2, chan_right_enc.size(),
                  chan_right_enc.data());
     } else {
-      std::vector<int16_t> chan_mono;
-      get_mono_stream(data, chan_mono);
-
+      std::vector<uint8_t> mono = mono_blend(
+          data, bytes_per_sample, number_of_required_samples_per_channel);
       if (left_cis_handle) {
-        lc3_encode(lc3_encoder_left, LC3_PCM_FORMAT_S16,
-                   (const int16_t*)chan_mono.data(), 1, chan_left_enc.size(),
-                   chan_left_enc.data());
+        lc3_encode(lc3_encoder_left, bits_per_sample, mono.data(), 1,
+                   chan_left_enc.size(), chan_left_enc.data());
       }
 
       if (right_cis_handle) {
-        lc3_encode(lc3_encoder_right, LC3_PCM_FORMAT_S16,
-                   (const int16_t*)chan_mono.data(), 1, chan_right_enc.size(),
-                   chan_right_enc.data());
+        lc3_encode(lc3_encoder_right, bits_per_sample, mono.data(), 1,
+                   chan_right_enc.size(), chan_right_enc.data());
       }
     }
 
@@ -1987,6 +2053,10 @@
     int dt_us = current_source_codec_config.data_interval_us;
     int af_hz = audio_framework_source_config.sample_rate;
     number_of_required_samples_per_channel = lc3_frame_samples(dt_us, af_hz);
+    lc3_pcm_format bits_per_sample =
+        bits_to_lc3_bits(audio_framework_source_config.bits_per_sample);
+    uint8_t bytes_per_sample =
+        bits_to_bytes_per_sample(audio_framework_source_config.bits_per_sample);
 
     if ((int)data.size() < (2 /* bytes per sample */ * num_channels *
                             number_of_required_samples_per_channel)) {
@@ -1998,21 +2068,19 @@
     if (num_channels == 1) {
       /* Since we always get two channels from framework, lets make it mono here
        */
-      std::vector<int16_t> chan_mono;
-      get_mono_stream(data, chan_mono);
+      std::vector<uint8_t> mono = mono_blend(
+          data, bytes_per_sample, number_of_required_samples_per_channel);
 
-      auto err = lc3_encode(lc3_encoder_left, LC3_PCM_FORMAT_S16,
-                            (const int16_t*)chan_mono.data(), 1, byte_count,
-                            chan_encoded.data());
+      auto err = lc3_encode(lc3_encoder_left, bits_per_sample, mono.data(), 1,
+                            byte_count, chan_encoded.data());
 
       if (err < 0) {
         LOG(ERROR) << " error while encoding, error code: " << +err;
       }
     } else {
-      lc3_encode(lc3_encoder_left, LC3_PCM_FORMAT_S16,
-                 (const int16_t*)data.data(), 2, byte_count,
-                 chan_encoded.data());
-      lc3_encode(lc3_encoder_right, LC3_PCM_FORMAT_S16,
+      lc3_encode(lc3_encoder_left, bits_per_sample, (const int16_t*)data.data(),
+                 2, byte_count, chan_encoded.data());
+      lc3_encode(lc3_encoder_right, bits_per_sample,
                  (const int16_t*)data.data() + 1, 2, byte_count,
                  chan_encoded.data() + byte_count);
     }
@@ -2212,6 +2280,8 @@
 
     int dt_us = current_sink_codec_config.data_interval_us;
     int af_hz = audio_framework_sink_config.sample_rate;
+    lc3_pcm_format bits_per_sample =
+        bits_to_lc3_bits(audio_framework_sink_config.bits_per_sample);
 
     int pcm_size;
     if (dt_us == 10000) {
@@ -2244,7 +2314,7 @@
     lc3_decoder_t decoder_to_use =
         is_left ? lc3_decoder_left : lc3_decoder_right;
 
-    err = lc3_decode(decoder_to_use, data, size, LC3_PCM_FORMAT_S16,
+    err = lc3_decode(decoder_to_use, data, size, bits_per_sample,
                      pcm_data_decoded.data(), 1 /* pitch */);
 
     if (err < 0) {
@@ -3623,10 +3693,6 @@
       << ", LE Audio Client requires Bluetooth Audio HAL V2.1 at least. Either "
          "disable LE Audio Profile, or update your HAL";
 
-  // TODO: The capability list should pass to the codec manager once it's ready
-  std::vector<::le_audio::set_configurations::AudioSetConfiguration>
-      capabilities = ::bluetooth::audio::le_audio::get_offload_capabilities();
-
   IsoManager::GetInstance()->Start();
 
   if (leAudioClientAudioSource == nullptr)
@@ -3642,7 +3708,7 @@
   instance = new LeAudioClientImpl(callbacks_, stateMachineCallbacks, initCb);
 
   IsoManager::GetInstance()->RegisterCigCallbacks(stateMachineHciCallbacks);
-  CodecManager::GetInstance()->Start(offloading_preference, capabilities);
+  CodecManager::GetInstance()->Start(offloading_preference);
 }
 
 void LeAudioClient::DebugDump(int fd) {
diff --git a/system/bta/le_audio/client_audio.h b/system/bta/le_audio/client_audio.h
index ae6614c..f812d12 100644
--- a/system/bta/le_audio/client_audio.h
+++ b/system/bta/le_audio/client_audio.h
@@ -96,6 +96,13 @@
              (data_interval_us == other.data_interval_us));
   }
 
+  bool operator==(const LeAudioCodecConfiguration& other) const {
+    return ((num_channels == other.num_channels) &&
+            (sample_rate == other.sample_rate) &&
+            (bits_per_sample == other.bits_per_sample) &&
+            (data_interval_us == other.data_interval_us));
+  }
+
   bool IsInvalid() {
     return (num_channels == 0) || (sample_rate == 0) ||
            (bits_per_sample == 0) || (data_interval_us == 0);
diff --git a/system/bta/le_audio/codec_manager.cc b/system/bta/le_audio/codec_manager.cc
index e34d19c..44b625f 100644
--- a/system/bta/le_audio/codec_manager.cc
+++ b/system/bta/le_audio/codec_manager.cc
@@ -45,8 +45,7 @@
 struct codec_manager_impl {
  public:
   codec_manager_impl(
-      const std::vector<btle_audio_codec_config_t>& offloading_preference,
-      const std::vector<AudioSetConfiguration>& adsp_capabilities) {
+      const std::vector<btle_audio_codec_config_t>& offloading_preference) {
     offload_enable_ = osi_property_get_bool(
                           "ro.bluetooth.leaudio_offload.supported", false) &&
                       !osi_property_get_bool(
@@ -71,7 +70,7 @@
                             kIsoDataPathPlatformDefault, {});
     btm_configure_data_path(btm_data_direction::CONTROLLER_TO_HOST,
                             kIsoDataPathPlatformDefault, {});
-    UpdateOffloadCapability(offloading_preference, adsp_capabilities);
+    UpdateOffloadCapability(offloading_preference);
     SetCodecLocation(CodecLocation::ADSP);
   }
   ~codec_manager_impl() {
@@ -218,8 +217,7 @@
   }
 
   void UpdateOffloadCapability(
-      const std::vector<btle_audio_codec_config_t>& offloading_preference,
-      const std::vector<AudioSetConfiguration>& adsp_capabilities) {
+      const std::vector<btle_audio_codec_config_t>& offloading_preference) {
     LOG(INFO) << __func__;
     std::unordered_set<uint8_t> offload_preference_set;
 
@@ -228,6 +226,10 @@
       return;
     }
 
+    std::vector<::le_audio::set_configurations::AudioSetConfiguration>
+        adsp_capabilities =
+            ::bluetooth::audio::le_audio::get_offload_capabilities();
+
     for (auto codec : offloading_preference) {
       auto it = btle_audio_codec_type_map_.find(codec.codec_type);
 
@@ -272,12 +274,10 @@
   impl(const CodecManager& codec_manager) : codec_manager_(codec_manager) {}
 
   void Start(
-      const std::vector<btle_audio_codec_config_t>& offloading_preference,
-      const std::vector<set_configurations::AudioSetConfiguration>&
-          adsp_capabilities) {
+      const std::vector<btle_audio_codec_config_t>& offloading_preference) {
     LOG_ASSERT(!codec_manager_impl_);
-    codec_manager_impl_ = std::make_unique<codec_manager_impl>(
-        offloading_preference, adsp_capabilities);
+    codec_manager_impl_ =
+        std::make_unique<codec_manager_impl>(offloading_preference);
   }
 
   void Stop() {
@@ -294,11 +294,8 @@
 CodecManager::CodecManager() : pimpl_(std::make_unique<impl>(*this)) {}
 
 void CodecManager::Start(
-    const std::vector<btle_audio_codec_config_t>& offloading_preference,
-    const std::vector<set_configurations::AudioSetConfiguration>&
-        adsp_capabilities) {
-  if (!pimpl_->IsRunning())
-    pimpl_->Start(offloading_preference, adsp_capabilities);
+    const std::vector<btle_audio_codec_config_t>& offloading_preference) {
+  if (!pimpl_->IsRunning()) pimpl_->Start(offloading_preference);
 }
 
 void CodecManager::Stop() {
diff --git a/system/bta/le_audio/codec_manager.h b/system/bta/le_audio/codec_manager.h
index d0416e1..1eed274 100644
--- a/system/bta/le_audio/codec_manager.h
+++ b/system/bta/le_audio/codec_manager.h
@@ -38,11 +38,8 @@
     static CodecManager* instance = new CodecManager();
     return instance;
   }
-  void Start(
-      const std::vector<bluetooth::le_audio::btle_audio_codec_config_t>&
-          offloading_preference,
-      const std::vector<::le_audio::set_configurations::AudioSetConfiguration>&
-          adsp_capabilities);
+  void Start(const std::vector<bluetooth::le_audio::btle_audio_codec_config_t>&
+                 offloading_preference);
   void Stop(void);
   virtual types::CodecLocation GetCodecLocation(void) const;
   virtual void UpdateActiveSourceAudioConfig(
diff --git a/system/bta/le_audio/devices.cc b/system/bta/le_audio/devices.cc
index 3d344ce..6731c6c 100644
--- a/system/bta/le_audio/devices.cc
+++ b/system/bta/le_audio/devices.cc
@@ -1081,6 +1081,9 @@
       /* Skip if device has ASE configured in this direction already */
       if (device->GetFirstActiveAseByDirection(ent.direction)) continue;
 
+      /* For the moment, we configure only connected devices. */
+      if (device->conn_id_ == GATT_INVALID_CONN_ID) continue;
+
       if (!device->ConfigureAses(ent, context_type, &active_ase_num,
                                  group_snk_audio_locations,
                                  group_src_audio_locations, reuse_cis_id))
diff --git a/system/bta/le_audio/devices_test.cc b/system/bta/le_audio/devices_test.cc
index 166b598..7628182 100644
--- a/system/bta/le_audio/devices_test.cc
+++ b/system/bta/le_audio/devices_test.cc
@@ -199,12 +199,18 @@
   /* Update those values, on any change of codec linked with content type */
   switch (context_type) {
     case LeAudioContextType::RINGTONE:
-    case LeAudioContextType::CONVERSATIONAL:
       if (id == Lc3SettingId::LC3_16_1 || id == Lc3SettingId::LC3_16_2)
         return true;
 
       break;
 
+    case LeAudioContextType::CONVERSATIONAL:
+      if (id == Lc3SettingId::LC3_16_1 || id == Lc3SettingId::LC3_16_2 ||
+          id == Lc3SettingId::LC3_32_2)
+        return true;
+
+      break;
+
     case LeAudioContextType::MEDIA:
       if (id == Lc3SettingId::LC3_16_1 || id == Lc3SettingId::LC3_16_2 ||
           id == Lc3SettingId::LC3_48_4 || id == Lc3SettingId::LC3_48_2 ||
diff --git a/system/bta/le_audio/le_audio_client_test.cc b/system/bta/le_audio/le_audio_client_test.cc
index f3db83a..6adc967 100644
--- a/system/bta/le_audio/le_audio_client_test.cc
+++ b/system/bta/le_audio/le_audio_client_test.cc
@@ -1032,10 +1032,12 @@
                          bool connect_through_csis = false,
                          bool new_device = true) {
     SetSampleDatabaseEarbudsValid(conn_id, addr, sink_audio_allocation,
-                                  source_audio_allocation, true, /*add_csis*/
-                                  true,                          /*add_cas*/
-                                  true,                          /*add_pacs*/
-                                  true,                          /*add_ascs*/
+                                  source_audio_allocation,
+                                  0x0004, /* source sample freq 16khz */
+                                  true,   /*add_csis*/
+                                  true,   /*add_cas*/
+                                  true,   /*add_pacs*/
+                                  true,   /*add_ascs*/
                                   group_size, rank);
     EXPECT_CALL(mock_client_callbacks_,
                 OnConnectionState(ConnectionState::CONNECTED, addr))
@@ -1066,12 +1068,13 @@
   void ConnectNonCsisDevice(const RawAddress& addr, uint16_t conn_id,
                             uint32_t sink_audio_allocation,
                             uint32_t source_audio_allocation) {
-    SetSampleDatabaseEarbudsValid(conn_id, addr, sink_audio_allocation,
-                                  source_audio_allocation, false, /*add_csis*/
-                                  true,                           /*add_cas*/
-                                  true,                           /*add_pacs*/
-                                  true,                           /*add_ascs*/
-                                  0, 0);
+    SetSampleDatabaseEarbudsValid(
+        conn_id, addr, sink_audio_allocation, source_audio_allocation, 0x0004,
+        /* source sample freq 16khz */ false, /*add_csis*/
+        true,                                 /*add_cas*/
+        true,                                 /*add_pacs*/
+        true,                                 /*add_ascs*/
+        0, 0);
     EXPECT_CALL(mock_client_callbacks_,
                 OnConnectionState(ConnectionState::CONNECTED, addr))
         .Times(1);
@@ -1358,6 +1361,7 @@
   void SetSampleDatabaseEarbudsValid(uint16_t conn_id, RawAddress addr,
                                      uint32_t sink_audio_allocation,
                                      uint32_t source_audio_allocation,
+                                     uint16_t sample_freq_mask = 0x0004,
                                      bool add_csis = true, bool add_cas = true,
                                      bool add_pacs = true, bool add_ascs = true,
                                      uint8_t set_size = 2, uint8_t rank = 1) {
@@ -1438,10 +1442,14 @@
       src_allocation[2] = (uint8_t)(source_audio_allocation >> 16);
       src_allocation[3] = (uint8_t)(source_audio_allocation >> 24);
 
+      uint8_t sample_freq[2];
+      sample_freq[0] = (uint8_t)(sample_freq_mask);
+      sample_freq[1] = (uint8_t)(sample_freq_mask >> 8);
+
       // Set pacs default read values
       ON_CALL(*peer_devices.at(conn_id)->pacs, OnReadCharacteristic(_, _, _))
           .WillByDefault(
-              [this, conn_id, snk_allocation, src_allocation](
+              [this, conn_id, snk_allocation, src_allocation, sample_freq](
                   uint16_t handle, GATT_READ_OP_CB cb, void* cb_data) {
                 auto& pacs = peer_devices.at(conn_id)->pacs;
                 std::vector<uint8_t> value;
@@ -1459,8 +1467,8 @@
                       0x10,
                       0x03,
                       0x01,
-                      0x04,
-                      0x00,
+                      sample_freq[0],
+                      sample_freq[1],
                       0x02,
                       0x02,
                       0x03,
@@ -1471,7 +1479,7 @@
                       0x04,
                       0x1E,
                       0x00,
-                      0x28,
+                      0x78,
                       0x00,
                       // Metadata Length
                       0x00,
@@ -1524,8 +1532,8 @@
                       0x10,
                       0x03,
                       0x01,
-                      0x04,
-                      0x00,
+                      sample_freq[0],
+                      sample_freq[1],
                       0x02,
                       0x02,
                       0x03,
@@ -1536,7 +1544,7 @@
                       0x04,
                       0x1E,
                       0x00,
-                      0x28,
+                      0x78,
                       0x00,
                       // Metadata Length
                       0x00,
@@ -1850,9 +1858,10 @@
   const RawAddress test_address0 = GetTestAddress(0);
   SetSampleDatabaseEarbudsValid(
       1, test_address0, codec_spec_conf::kLeAudioLocationStereo,
-      codec_spec_conf::kLeAudioLocationStereo, true, /*add_csis*/
-      true,                                          /*add_cas*/
-      false,                                         /*add_pacs*/
+      codec_spec_conf::kLeAudioLocationStereo, 0x0004,
+      /* source sample freq 16khz */ true, /*add_csis*/
+      true,                                /*add_cas*/
+      false,                               /*add_pacs*/
       true /*add_ascs*/);
   EXPECT_CALL(mock_client_callbacks_,
               OnConnectionState(ConnectionState::DISCONNECTED, test_address0))
@@ -1865,9 +1874,10 @@
   const RawAddress test_address0 = GetTestAddress(0);
   SetSampleDatabaseEarbudsValid(
       1, test_address0, codec_spec_conf::kLeAudioLocationStereo,
-      codec_spec_conf::kLeAudioLocationStereo, true, /*add_csis*/
-      true,                                          /*add_cas*/
-      true,                                          /*add_pacs*/
+      codec_spec_conf::kLeAudioLocationStereo, 0x0004,
+      /* source sample freq 16khz */ true, /*add_csis*/
+      true,                                /*add_cas*/
+      true,                                /*add_pacs*/
       false /*add_ascs*/);
   EXPECT_CALL(mock_client_callbacks_,
               OnConnectionState(ConnectionState::DISCONNECTED, test_address0))
@@ -1881,9 +1891,10 @@
   uint16_t conn_id = 1;
   SetSampleDatabaseEarbudsValid(
       conn_id, test_address0, codec_spec_conf::kLeAudioLocationStereo,
-      codec_spec_conf::kLeAudioLocationStereo, true, /*add_csis*/
-      false,                                         /*add_cas*/
-      true,                                          /*add_pacs*/
+      codec_spec_conf::kLeAudioLocationStereo, 0x0004,
+      /* source sample freq 16khz */ true, /*add_csis*/
+      false,                               /*add_cas*/
+      true,                                /*add_pacs*/
       true /*add_ascs*/);
 
   EXPECT_CALL(mock_client_callbacks_,
@@ -1896,9 +1907,10 @@
   const RawAddress test_address0 = GetTestAddress(0);
   SetSampleDatabaseEarbudsValid(
       1, test_address0, codec_spec_conf::kLeAudioLocationStereo,
-      codec_spec_conf::kLeAudioLocationStereo, false, /*add_csis*/
-      true,                                           /*add_cas*/
-      true,                                           /*add_pacs*/
+      codec_spec_conf::kLeAudioLocationStereo, 0x0004,
+      /* source sample freq 16khz */ false, /*add_csis*/
+      true,                                 /*add_cas*/
+      true,                                 /*add_pacs*/
       true /*add_ascs*/);
   EXPECT_CALL(mock_client_callbacks_,
               OnConnectionState(ConnectionState::CONNECTED, test_address0))
@@ -2039,19 +2051,21 @@
   const RawAddress test_address0 = GetTestAddress(0);
   SetSampleDatabaseEarbudsValid(
       1, test_address0, codec_spec_conf::kLeAudioLocationFrontLeft,
-      codec_spec_conf::kLeAudioLocationFrontLeft, true, /*add_csis*/
-      true,                                             /*add_cas*/
-      true,                                             /*add_pacs*/
-      true,                                             /*add_ascs*/
+      codec_spec_conf::kLeAudioLocationFrontLeft, 0x0004,
+      /* source sample freq 16khz */ true, /*add_csis*/
+      true,                                /*add_cas*/
+      true,                                /*add_pacs*/
+      true,                                /*add_ascs*/
       group_size, 1);
 
   const RawAddress test_address1 = GetTestAddress(1);
   SetSampleDatabaseEarbudsValid(
       2, test_address1, codec_spec_conf::kLeAudioLocationFrontRight,
-      codec_spec_conf::kLeAudioLocationFrontRight, true, /*add_csis*/
-      true,                                              /*add_cas*/
-      true,                                              /*add_pacs*/
-      true,                                              /*add_ascs*/
+      codec_spec_conf::kLeAudioLocationFrontRight, 0x0004,
+      /* source sample freq 16khz */ true, /*add_csis*/
+      true,                                /*add_cas*/
+      true,                                /*add_pacs*/
+      true,                                /*add_ascs*/
       group_size, 2);
 
   // Load devices from the storage when storage API is called
@@ -2134,10 +2148,11 @@
   const RawAddress test_address0 = GetTestAddress(0);
   SetSampleDatabaseEarbudsValid(
       1, test_address0, codec_spec_conf::kLeAudioLocationFrontLeft,
-      codec_spec_conf::kLeAudioLocationFrontLeft, true, /*add_csis*/
-      true,                                             /*add_cas*/
-      true,                                             /*add_pacs*/
-      true,                                             /*add_ascs*/
+      codec_spec_conf::kLeAudioLocationFrontLeft, 0x0004,
+      /* source sample freq 16khz */ true, /*add_csis*/
+      true,                                /*add_cas*/
+      true,                                /*add_pacs*/
+      true,                                /*add_ascs*/
       group_size, 1);
 
   ON_CALL(mock_groups_module_, GetGroupId(test_address0, _))
@@ -2149,10 +2164,11 @@
   const RawAddress test_address1 = GetTestAddress(1);
   SetSampleDatabaseEarbudsValid(
       2, test_address1, codec_spec_conf::kLeAudioLocationFrontRight,
-      codec_spec_conf::kLeAudioLocationFrontRight, true, /*add_csis*/
-      true,                                              /*add_cas*/
-      true,                                              /*add_pacs*/
-      true,                                              /*add_ascs*/
+      codec_spec_conf::kLeAudioLocationFrontRight, 0x0004,
+      /* source sample freq 16khz */ true, /*add_csis*/
+      true,                                /*add_cas*/
+      true,                                /*add_pacs*/
+      true,                                /*add_ascs*/
       group_size, 2);
 
   ON_CALL(mock_groups_module_, GetGroupId(test_address1, _))
@@ -2469,9 +2485,9 @@
 
   SetSampleDatabaseEarbudsValid(
       1, test_address0, codec_spec_conf::kLeAudioLocationStereo,
-      codec_spec_conf::kLeAudioLocationStereo, false /*add_csis*/,
-      true /*add_cas*/, true /*add_pacs*/, true /*add_ascs*/, 1 /*set_size*/,
-      0 /*rank*/);
+      codec_spec_conf::kLeAudioLocationStereo, 0x0004,
+      /* source sample freq 16khz */ false /*add_csis*/, true /*add_cas*/,
+      true /*add_pacs*/, true /*add_ascs*/, 1 /*set_size*/, 0 /*rank*/);
   EXPECT_CALL(mock_client_callbacks_,
               OnConnectionState(ConnectionState::CONNECTED, test_address0))
       .Times(1);
@@ -2534,9 +2550,9 @@
 
   SetSampleDatabaseEarbudsValid(
       1, test_address0, codec_spec_conf::kLeAudioLocationStereo,
-      codec_spec_conf::kLeAudioLocationStereo, false /*add_csis*/,
-      true /*add_cas*/, true /*add_pacs*/, true /*add_ascs*/, 1 /*set_size*/,
-      0 /*rank*/);
+      codec_spec_conf::kLeAudioLocationStereo, 0x0004,
+      /* source sample freq 16khz */ false /*add_csis*/, true /*add_cas*/,
+      true /*add_pacs*/, true /*add_ascs*/, 1 /*set_size*/, 0 /*rank*/);
   EXPECT_CALL(mock_client_callbacks_,
               OnConnectionState(ConnectionState::CONNECTED, test_address0))
       .Times(1);
@@ -2594,9 +2610,9 @@
 
   SetSampleDatabaseEarbudsValid(
       1, test_address0, codec_spec_conf::kLeAudioLocationStereo,
-      codec_spec_conf::kLeAudioLocationStereo, false /*add_csis*/,
-      true /*add_cas*/, true /*add_pacs*/, true /*add_ascs*/, 1 /*set_size*/,
-      0 /*rank*/);
+      codec_spec_conf::kLeAudioLocationStereo, 0x0004,
+      /* source sample freq 16khz */ false /*add_csis*/, true /*add_cas*/,
+      true /*add_pacs*/, true /*add_ascs*/, 1 /*set_size*/, 0 /*rank*/);
   EXPECT_CALL(mock_client_callbacks_,
               OnConnectionState(ConnectionState::CONNECTED, test_address0))
       .Times(1);
@@ -3008,5 +3024,35 @@
   Mock::VerifyAndClearExpectations(&mock_client_callbacks_);
 }
 
+TEST_F(UnicastTest, TwoEarbudsWithSourceSupporting32kHz) {
+  const RawAddress test_address0 = GetTestAddress(0);
+  int group_id = 0;
+  SetSampleDatabaseEarbudsValid(
+      1, test_address0, codec_spec_conf::kLeAudioLocationStereo,
+      codec_spec_conf::kLeAudioLocationStereo, 0x0024,
+      /* source sample freq 32/16khz */ true, /*add_csis*/
+      true,                                   /*add_cas*/
+      true,                                   /*add_pacs*/
+      true /*add_ascs*/);
+  EXPECT_CALL(mock_client_callbacks_,
+              OnConnectionState(ConnectionState::CONNECTED, test_address0))
+      .Times(1);
+  ConnectLeAudio(test_address0);
+
+  // LeAudioCodecConfiguration received_af_sink_config;
+  const LeAudioCodecConfiguration expected_af_sink_config = {
+      .num_channels = 2,
+      .sample_rate = bluetooth::audio::le_audio::kSampleRate32000,
+      .bits_per_sample = bluetooth::audio::le_audio::kBitsPerSample16,
+      .data_interval_us = LeAudioCodecConfiguration::kInterval10000Us,
+  };
+
+  // Audio sessions are started only when device gets active
+  EXPECT_CALL(*mock_unicast_audio_source_, Start(_, _)).Times(1);
+  EXPECT_CALL(*mock_audio_sink_, Start(expected_af_sink_config, _)).Times(1);
+  LeAudioClient::Get()->GroupSetActive(group_id);
+  SyncOnMainLoop();
+}
+
 }  // namespace
 }  // namespace le_audio
diff --git a/system/bta/le_audio/le_audio_types.h b/system/bta/le_audio/le_audio_types.h
index 629e352..b5067d4 100644
--- a/system/bta/le_audio/le_audio_types.h
+++ b/system/bta/le_audio/le_audio_types.h
@@ -173,6 +173,7 @@
 constexpr uint16_t kLeAudioCodecLC3FrameLen30 = 30;
 constexpr uint16_t kLeAudioCodecLC3FrameLen40 = 40;
 constexpr uint16_t kLeAudioCodecLC3FrameLen60 = 60;
+constexpr uint16_t kLeAudioCodecLC3FrameLen80 = 80;
 constexpr uint16_t kLeAudioCodecLC3FrameLen120 = 120;
 
 }  // namespace codec_spec_conf
diff --git a/system/bta/le_audio/mock_codec_manager.cc b/system/bta/le_audio/mock_codec_manager.cc
index 6b2dd3d..d433b5f 100644
--- a/system/bta/le_audio/mock_codec_manager.cc
+++ b/system/bta/le_audio/mock_codec_manager.cc
@@ -63,9 +63,7 @@
 
 void CodecManager::Start(
     const std::vector<bluetooth::le_audio::btle_audio_codec_config_t>&
-        offloading_preference,
-    const std::vector<set_configurations::AudioSetConfiguration>&
-        adsp_capabilities) {
+        offloading_preference) {
   // It is needed here as CodecManager which is a singleton creates it, but in
   // this mock we want to destroy and recreate the mock on each test case.
   if (!pimpl_) {
diff --git a/system/bta/le_audio/state_machine_test.cc b/system/bta/le_audio/state_machine_test.cc
index ba8d267..ff33fd3 100644
--- a/system/bta/le_audio/state_machine_test.cc
+++ b/system/bta/le_audio/state_machine_test.cc
@@ -393,9 +393,7 @@
     ASSERT_NE(codec_manager_, nullptr);
     std::vector<bluetooth::le_audio::btle_audio_codec_config_t>
         mock_offloading_preference(0);
-    std::vector<set_configurations::AudioSetConfiguration>
-        mock_adsp_capabilities(0);
-    codec_manager_->Start(mock_offloading_preference, mock_adsp_capabilities);
+    codec_manager_->Start(mock_offloading_preference);
     mock_codec_manager_ = MockCodecManager::GetInstance();
     ASSERT_NE(mock_codec_manager_, nullptr);
     ON_CALL(*mock_codec_manager_, GetCodecLocation())
diff --git a/system/bta/vc/vc.cc b/system/bta/vc/vc.cc
index 4ecbb8e..d81f00c 100644
--- a/system/bta/vc/vc.cc
+++ b/system/bta/vc/vc.cc
@@ -32,6 +32,8 @@
 #include "bta_vc_api.h"
 #include "btif_storage.h"
 #include "devices.h"
+#include "gd/common/strings.h"
+#include "osi/include/log.h"
 #include "osi/include/osi.h"
 #include "types/bluetooth/uuid.h"
 #include "types/raw_address.h"
@@ -711,6 +713,61 @@
                                      is_autonomous, opcode, arguments, devices);
   }
 
+  void MuteUnmute(std::variant<RawAddress, int> addr_or_group_id, bool mute) {
+    std::vector<uint8_t> arg;
+
+    uint8_t opcode = mute ? kControlPointOpcodeMute : kControlPointOpcodeUnmute;
+
+    if (std::holds_alternative<RawAddress>(addr_or_group_id)) {
+      LOG_DEBUG("Address: %s: ",
+                (std::get<RawAddress>(addr_or_group_id)).ToString().c_str());
+      std::vector<RawAddress> devices = {
+          std::get<RawAddress>(addr_or_group_id)};
+
+      PrepareVolumeControlOperation(devices, bluetooth::groups::kGroupUnknown,
+                                    false, opcode, arg);
+    } else {
+      /* Handle group change */
+      auto group_id = std::get<int>(addr_or_group_id);
+      LOG_DEBUG("group: %d", group_id);
+      auto csis_api = CsisClient::Get();
+      if (!csis_api) {
+        LOG(ERROR) << __func__ << " Csis is not there";
+        return;
+      }
+
+      auto devices = csis_api->GetDeviceList(group_id);
+      for (auto it = devices.begin(); it != devices.end();) {
+        auto dev = volume_control_devices_.FindByAddress(*it);
+        if (!dev || !dev->IsConnected()) {
+          it = devices.erase(it);
+        } else {
+          it++;
+        }
+      }
+
+      if (devices.empty()) {
+        LOG(ERROR) << __func__ << " group id : " << group_id
+                   << " is not connected? ";
+        return;
+      }
+
+      PrepareVolumeControlOperation(devices, group_id, false, opcode, arg);
+    }
+
+    StartQueueOperation();
+  }
+
+  void Mute(std::variant<RawAddress, int> addr_or_group_id) override {
+    LOG_DEBUG();
+    MuteUnmute(addr_or_group_id, true /* mute */);
+  }
+
+  void UnMute(std::variant<RawAddress, int> addr_or_group_id) override {
+    LOG_DEBUG();
+    MuteUnmute(addr_or_group_id, false /* mute */);
+  }
+
   void SetVolume(std::variant<RawAddress, int> addr_or_group_id,
                  uint8_t volume) override {
     DLOG(INFO) << __func__ << " vol: " << +volume;
diff --git a/system/bta/vc/vc_test.cc b/system/bta/vc/vc_test.cc
index e90ceee..7225f59 100644
--- a/system/bta/vc/vc_test.cc
+++ b/system/bta/vc/vc_test.cc
@@ -919,6 +919,20 @@
   VolumeControl::Get()->SetVolume(test_address, 0x10);
 }
 
+TEST_F(VolumeControlValueSetTest, test_mute) {
+  std::vector<uint8_t> mute({0x06, 0x00});
+  EXPECT_CALL(gatt_queue,
+              WriteCharacteristic(conn_id, 0x0024, mute, GATT_WRITE, _, _));
+  VolumeControl::Get()->Mute(test_address);
+}
+
+TEST_F(VolumeControlValueSetTest, test_unmute) {
+  std::vector<uint8_t> unmute({0x05, 0x00});
+  EXPECT_CALL(gatt_queue,
+              WriteCharacteristic(conn_id, 0x0024, unmute, GATT_WRITE, _, _));
+  VolumeControl::Get()->UnMute(test_address);
+}
+
 TEST_F(VolumeControlValueSetTest, test_set_ext_audio_out_volume_offset) {
   std::vector<uint8_t> expected_data({0x01, 0x00, 0x34, 0x12});
   EXPECT_CALL(gatt_queue, WriteCharacteristic(conn_id, 0x0088, expected_data,
diff --git a/system/btif/src/btif_av.cc b/system/btif/src/btif_av.cc
index 8ae8db8..5099060 100644
--- a/system/btif/src/btif_av.cc
+++ b/system/btif/src/btif_av.cc
@@ -3469,7 +3469,9 @@
           btif_av_source.ActivePeer().ToString().c_str());
   for (auto it : btif_av_source.Peers()) {
     const BtifAvPeer* peer = it.second;
-    btif_debug_av_peer_dump(fd, *peer);
+    if (peer != nullptr) {
+      btif_debug_av_peer_dump(fd, *peer);
+    }
   }
 }
 
diff --git a/system/btif/src/btif_hf.cc b/system/btif/src/btif_hf.cc
index cc09d24..995ba83 100644
--- a/system/btif/src/btif_hf.cc
+++ b/system/btif/src/btif_hf.cc
@@ -27,12 +27,16 @@
 
 #define LOG_TAG "bt_btif_hf"
 
+#include <base/logging.h>
+#include <frameworks/proto_logging/stats/enums/bluetooth/enums.pb.h>
+
 #include <cstdint>
 #include <string>
 
 #include "bta/include/bta_ag_api.h"
 #include "bta/include/utl.h"
 #include "btif/include/btif_common.h"
+#include "btif/include/btif_metrics_logging.h"
 #include "btif/include/btif_profile_queue.h"
 #include "btif/include/btif_util.h"
 #include "common/metrics.h"
@@ -44,8 +48,6 @@
 #include "stack/include/btm_api.h"
 #include "types/raw_address.h"
 
-#include <base/logging.h>
-
 namespace {
 constexpr char kBtmLogTag[] = "HFP";
 }
@@ -339,6 +341,10 @@
                          << ", report disconnect state for p_data bda.";
             bt_hf_callbacks->ConnectionStateCallback(
                 BTHF_CONNECTION_STATE_DISCONNECTED, &(p_data->open.bd_addr));
+            log_counter_metrics_btif(
+                android::bluetooth::CodePathCounterKeyEnum::
+                    HFP_COLLISON_AT_AG_OPEN,
+                1);
           }
           break;
         }
@@ -358,6 +364,9 @@
           bt_hf_callbacks->ConnectionStateCallback(
               BTHF_CONNECTION_STATE_DISCONNECTED,
               &(btif_hf_cb[idx].connected_bda));
+          log_counter_metrics_btif(android::bluetooth::CodePathCounterKeyEnum::
+                                       HFP_COLLISON_AT_CONNECTING,
+                                   1);
           reset_control_block(&btif_hf_cb[idx]);
           btif_queue_advance();
         }
@@ -387,6 +396,9 @@
         reset_control_block(&btif_hf_cb[idx]);
         bt_hf_callbacks->ConnectionStateCallback(btif_hf_cb[idx].state,
                                                  &connected_bda);
+        log_counter_metrics_btif(android::bluetooth::CodePathCounterKeyEnum::
+                                     HFP_SELF_INITIATED_AG_FAILED,
+                                 1);
         btif_queue_advance();
       }
       break;
@@ -406,6 +418,9 @@
                                                &connected_bda);
       if (failed_to_setup_slc) {
         LOG(ERROR) << __func__ << ": failed to setup SLC for " << connected_bda;
+        log_counter_metrics_btif(
+            android::bluetooth::CodePathCounterKeyEnum::HFP_SLC_SETUP_FAILED,
+            1);
         btif_queue_advance();
       }
       break;
@@ -1075,7 +1090,7 @@
   if (index == 0) {
     ag_res.ok_flag = BTA_AG_OK_DONE;
   } else {
-    std::string cell_number(number);
+    std::string cell_number(number ? number : "");
     BTIF_TRACE_EVENT(
         "clcc_response: [%d] dir %d state %d mode %d number = %s type = %d",
         index, dir, state, mode, PRIVATE_CELL(cell_number), type);
diff --git a/system/btif/src/btif_storage.cc b/system/btif/src/btif_storage.cc
index 338fb24..6c94377 100644
--- a/system/btif/src/btif_storage.cc
+++ b/system/btif/src/btif_storage.cc
@@ -486,7 +486,12 @@
           }
         }
         bt_linkkey_file_found = true;
-        p_bonded_devices->devices[p_bonded_devices->num_devices++] = bd_addr;
+        if (p_bonded_devices->num_devices < BTM_SEC_MAX_DEVICE_RECORDS) {
+          p_bonded_devices->devices[p_bonded_devices->num_devices++] = bd_addr;
+        } else {
+          BTIF_TRACE_WARNING("%s: exceed the max number of bonded devices",
+                             __func__);
+        }
       } else {
         bt_linkkey_file_found = false;
       }
@@ -1369,7 +1374,12 @@
 
     // Fill in the bonded devices
     if (device_added) {
-      p_bonded_devices->devices[p_bonded_devices->num_devices++] = bd_addr;
+      if (p_bonded_devices->num_devices < BTM_SEC_MAX_DEVICE_RECORDS) {
+        p_bonded_devices->devices[p_bonded_devices->num_devices++] = bd_addr;
+      } else {
+        BTIF_TRACE_WARNING("%s: exceed the max number of bonded devices",
+                           __func__);
+      }
       btif_gatts_add_bonded_dev_from_nv(bd_addr);
     }
 
diff --git a/system/btif/src/btif_vc.cc b/system/btif/src/btif_vc.cc
index 28aaa34..00b6b8e 100644
--- a/system/btif/src/btif_vc.cc
+++ b/system/btif/src/btif_vc.cc
@@ -143,6 +143,20 @@
                                       std::move(addr_or_group_id), volume));
   }
 
+  void Mute(std::variant<RawAddress, int> addr_or_group_id) override {
+    DVLOG(2) << __func__;
+    do_in_main_thread(
+        FROM_HERE, Bind(&VolumeControl::Mute, Unretained(VolumeControl::Get()),
+                        std::move(addr_or_group_id)));
+  }
+
+  void Unmute(std::variant<RawAddress, int> addr_or_group_id) override {
+    DVLOG(2) << __func__;
+    do_in_main_thread(FROM_HERE, Bind(&VolumeControl::UnMute,
+                                      Unretained(VolumeControl::Get()),
+                                      std::move(addr_or_group_id)));
+  }
+
   void RemoveDevice(const RawAddress& address) override {
     DVLOG(2) << __func__ << " address: " << address;
 
diff --git a/system/gd/.clang-format b/system/gd/.clang-format
index edf31fe..cf9a43a 100644
--- a/system/gd/.clang-format
+++ b/system/gd/.clang-format
@@ -22,7 +22,7 @@
 BasedOnStyle: Google
 CommentPragmas: NOLINT:.*
 DerivePointerAlignment: false
-ColumnLimit: 120
+ColumnLimit: 100
 AllowShortFunctionsOnASingleLine: Empty
 ConstructorInitializerAllOnOneLineOrOnePerLine: true
 BreakConstructorInitializers: BeforeColon
diff --git a/system/gd/Android.bp b/system/gd/Android.bp
index b6e3add..a6ebfe4 100644
--- a/system/gd/Android.bp
+++ b/system/gd/Android.bp
@@ -312,9 +312,8 @@
         "gd_clang_coverage_bin",
     ],
     host_supported: true,
-    test_options: {
-        unit_test: true,
-    },
+    // TODO(b/231993739): Reenable isolated:true by deleting the explicit disable below
+    isolated: false,
     target: {
         linux: {
             srcs: [
diff --git a/system/gd/BUILD.gn b/system/gd/BUILD.gn
index 11aa01d..0641fcc 100644
--- a/system/gd/BUILD.gn
+++ b/system/gd/BUILD.gn
@@ -63,6 +63,7 @@
   include_dirs = [ "." ]
   configs += [ ":gd_defaults" ]
   deps = [
+    "//bt/system/gd/rust/common:libbt_keystore_cc",
     "//bt/system/gd/rust/topshim:libbluetooth_topshim",
     "//bt/system/gd/rust/shim:libbluetooth_rust_interop",
     "//bt/system/gd:BluetoothGeneratedPackets_h",
diff --git a/system/gd/hci/acl_manager/le_impl_test.cc b/system/gd/hci/acl_manager/le_impl_test.cc
index 1f3c17f..d87ebcb 100644
--- a/system/gd/hci/acl_manager/le_impl_test.cc
+++ b/system/gd/hci/acl_manager/le_impl_test.cc
@@ -88,12 +88,12 @@
 
     void EnqueueCommand(
         std::unique_ptr<T> command, common::ContextualOnceCallback<void(CommandCompleteView)> on_complete) override {
-      hci_.EnqueueCommand(move(command), std::move(on_complete));
+      hci_.EnqueueCommand(std::move(command), std::move(on_complete));
     }
 
     void EnqueueCommand(
         std::unique_ptr<T> command, common::ContextualOnceCallback<void(CommandStatusView)> on_status) override {
-      hci_.EnqueueCommand(move(command), std::move(on_status));
+      hci_.EnqueueCommand(std::move(command), std::move(on_status));
     }
     HciLayer& hci_;
   };
diff --git a/system/gd/hci/hci_layer.cc b/system/gd/hci/hci_layer.cc
index 57d7e55..0d3b77d 100644
--- a/system/gd/hci/hci_layer.cc
+++ b/system/gd/hci/hci_layer.cc
@@ -44,7 +44,6 @@
 using hci::ResetCompleteView;
 using os::Alarm;
 using os::Handler;
-using std::move;
 using std::unique_ptr;
 
 static void fail_if_reset_complete_not_success(CommandCompleteView complete) {
@@ -61,12 +60,18 @@
 class CommandQueueEntry {
  public:
   CommandQueueEntry(
-      unique_ptr<CommandBuilder> command_packet, ContextualOnceCallback<void(CommandCompleteView)> on_complete_function)
-      : command(move(command_packet)), waiting_for_status_(false), on_complete(move(on_complete_function)) {}
+      unique_ptr<CommandBuilder> command_packet,
+      ContextualOnceCallback<void(CommandCompleteView)> on_complete_function)
+      : command(std::move(command_packet)),
+        waiting_for_status_(false),
+        on_complete(std::move(on_complete_function)) {}
 
   CommandQueueEntry(
-      unique_ptr<CommandBuilder> command_packet, ContextualOnceCallback<void(CommandStatusView)> on_status_function)
-      : command(move(command_packet)), waiting_for_status_(true), on_status(move(on_status_function)) {}
+      unique_ptr<CommandBuilder> command_packet,
+      ContextualOnceCallback<void(CommandStatusView)> on_status_function)
+      : command(std::move(command_packet)),
+        waiting_for_status_(true),
+        on_status(std::move(on_status_function)) {}
 
   unique_ptr<CommandBuilder> command;
   unique_ptr<CommandView> command_view;
@@ -139,7 +144,7 @@
 
   template <typename TResponse>
   void enqueue_command(unique_ptr<CommandBuilder> command, ContextualOnceCallback<void(TResponse)> on_response) {
-    command_queue_.emplace_back(move(command), move(on_response));
+    command_queue_.emplace_back(std::move(command), std::move(on_response));
     send_next_command();
   }
 
@@ -174,26 +179,36 @@
     }
     bool is_status = logging_id == "status";
 
-    ASSERT_LOG(!command_queue_.empty(), "Unexpected %s event with OpCode 0x%02hx (%s)", logging_id.c_str(), op_code,
-               OpCodeText(op_code).c_str());
+    ASSERT_LOG(
+        !command_queue_.empty(),
+        "Unexpected %s event with OpCode 0x%02hx (%s)",
+        logging_id.c_str(),
+        op_code,
+        OpCodeText(op_code).c_str());
     if (waiting_command_ == OpCode::CONTROLLER_DEBUG_INFO && op_code != OpCode::CONTROLLER_DEBUG_INFO) {
       LOG_ERROR("Discarding event that came after timeout 0x%02hx (%s)", op_code, OpCodeText(op_code).c_str());
       return;
     }
-    ASSERT_LOG(waiting_command_ == op_code, "Waiting for 0x%02hx (%s), got 0x%02hx (%s)", waiting_command_,
-               OpCodeText(waiting_command_).c_str(), op_code, OpCodeText(op_code).c_str());
+    ASSERT_LOG(
+        waiting_command_ == op_code,
+        "Waiting for 0x%02hx (%s), got 0x%02hx (%s)",
+        waiting_command_,
+        OpCodeText(waiting_command_).c_str(),
+        op_code,
+        OpCodeText(op_code).c_str());
 
     bool is_vendor_specific = static_cast<int>(op_code) & (0x3f << 10);
     CommandStatusView status_view = CommandStatusView::Create(event);
     if (is_vendor_specific && (is_status && !command_queue_.front().waiting_for_status_) &&
         (status_view.IsValid() && status_view.GetStatus() == ErrorCode::UNKNOWN_HCI_COMMAND)) {
-      // If this is a command status of a vendor specific command, and command complete is expected, we can't treat
-      // this as hard failure since we have no way of probing this lack of support at earlier time. Instead we let
-      // the command complete handler handle a empty Command Complete packet, which will be interpreted as invalid
-      // response.
+      // If this is a command status of a vendor specific command, and command complete is expected,
+      // we can't treat this as hard failure since we have no way of probing this lack of support at
+      // earlier time. Instead we let the command complete handler handle a empty Command Complete
+      // packet, which will be interpreted as invalid response.
       CommandCompleteView command_complete_view = CommandCompleteView::Create(
           EventView::Create(PacketView<kLittleEndian>(std::make_shared<std::vector<uint8_t>>(std::vector<uint8_t>()))));
-      command_queue_.front().GetCallback<CommandCompleteView>()->Invoke(move(command_complete_view));
+      command_queue_.front().GetCallback<CommandCompleteView>()->Invoke(
+          std::move(command_complete_view));
     } else {
       ASSERT_LOG(
           command_queue_.front().waiting_for_status_ == is_status,
@@ -202,7 +217,7 @@
           OpCodeText(op_code).c_str(),
           logging_id.c_str());
 
-      command_queue_.front().GetCallback<TResponse>()->Invoke(move(response_view));
+      command_queue_.front().GetCallback<TResponse>()->Invoke(std::move(response_view));
     }
 
     command_queue_.pop_front();
@@ -275,8 +290,11 @@
         "Can not register handler for %02hhx (%s)",
         EventCode::LE_META_EVENT,
         EventCodeText(EventCode::LE_META_EVENT).c_str());
-    ASSERT_LOG(event_handlers_.count(event) == 0, "Can not register a second handler for %02hhx (%s)", event,
-               EventCodeText(event).c_str());
+    ASSERT_LOG(
+        event_handlers_.count(event) == 0,
+        "Can not register a second handler for %02hhx (%s)",
+        event,
+        EventCodeText(event).c_str());
     event_handlers_[event] = handler;
   }
 
@@ -298,8 +316,11 @@
   }
 
   void register_le_event(SubeventCode event, ContextualCallback<void(LeMetaEventView)> handler) {
-    ASSERT_LOG(subevent_handlers_.count(event) == 0, "Can not register a second handler for %02hhx (%s)", event,
-               SubeventCodeText(event).c_str());
+    ASSERT_LOG(
+        subevent_handlers_.count(event) == 0,
+        "Can not register a second handler for %02hhx (%s)",
+        event,
+        SubeventCodeText(event).c_str());
     subevent_handlers_[event] = handler;
   }
 
@@ -336,22 +357,28 @@
       // BT Core spec 5.2 (Volume 4, Part E section 4.4) allows anytime
       // COMMAND_COMPLETE and COMMAND_STATUS with opcode 0x0 for flow control
       if (event_code == EventCode::COMMAND_COMPLETE) {
-          auto view = CommandCompleteView::Create(event);
-          ASSERT(view.IsValid());
-          auto op_code = view.GetCommandOpCode();
-          ASSERT_LOG(op_code == OpCode::NONE,
+        auto view = CommandCompleteView::Create(event);
+        ASSERT(view.IsValid());
+        auto op_code = view.GetCommandOpCode();
+        ASSERT_LOG(
+            op_code == OpCode::NONE,
             "Received %s event with OpCode 0x%02hx (%s) without a waiting command"
             "(is the HAL sending commands, but not handling the events?)",
-            EventCodeText(event_code).c_str(), op_code, OpCodeText(op_code).c_str());
+            EventCodeText(event_code).c_str(),
+            op_code,
+            OpCodeText(op_code).c_str());
       }
       if (event_code == EventCode::COMMAND_STATUS) {
-          auto view = CommandStatusView::Create(event);
-          ASSERT(view.IsValid());
-          auto op_code = view.GetCommandOpCode();
-          ASSERT_LOG(op_code == OpCode::NONE,
+        auto view = CommandStatusView::Create(event);
+        ASSERT(view.IsValid());
+        auto op_code = view.GetCommandOpCode();
+        ASSERT_LOG(
+            op_code == OpCode::NONE,
             "Received %s event with OpCode 0x%02hx (%s) without a waiting command"
             "(is the HAL sending commands, but not handling the events?)",
-            EventCodeText(event_code).c_str(), op_code, OpCodeText(op_code).c_str());
+            EventCodeText(event_code).c_str(),
+            op_code,
+            OpCodeText(op_code).c_str());
       }
       std::unique_ptr<CommandView> no_waiting_command{nullptr};
       log_hci_event(no_waiting_command, event, module_.GetDependency<storage::StorageModule>());
@@ -423,25 +450,28 @@
   void hciEventReceived(hal::HciPacket event_bytes) override {
     auto packet = packet::PacketView<packet::kLittleEndian>(std::make_shared<std::vector<uint8_t>>(event_bytes));
     EventView event = EventView::Create(packet);
-    module_.CallOn(module_.impl_, &impl::on_hci_event, move(event));
+    module_.CallOn(module_.impl_, &impl::on_hci_event, std::move(event));
   }
 
   void aclDataReceived(hal::HciPacket data_bytes) override {
-    auto packet = packet::PacketView<packet::kLittleEndian>(std::make_shared<std::vector<uint8_t>>(move(data_bytes)));
+    auto packet = packet::PacketView<packet::kLittleEndian>(
+        std::make_shared<std::vector<uint8_t>>(std::move(data_bytes)));
     auto acl = std::make_unique<AclView>(AclView::Create(packet));
-    module_.impl_->incoming_acl_buffer_.Enqueue(move(acl), module_.GetHandler());
+    module_.impl_->incoming_acl_buffer_.Enqueue(std::move(acl), module_.GetHandler());
   }
 
   void scoDataReceived(hal::HciPacket data_bytes) override {
-    auto packet = packet::PacketView<packet::kLittleEndian>(std::make_shared<std::vector<uint8_t>>(move(data_bytes)));
+    auto packet = packet::PacketView<packet::kLittleEndian>(
+        std::make_shared<std::vector<uint8_t>>(std::move(data_bytes)));
     auto sco = std::make_unique<ScoView>(ScoView::Create(packet));
-    module_.impl_->incoming_sco_buffer_.Enqueue(move(sco), module_.GetHandler());
+    module_.impl_->incoming_sco_buffer_.Enqueue(std::move(sco), module_.GetHandler());
   }
 
   void isoDataReceived(hal::HciPacket data_bytes) override {
-    auto packet = packet::PacketView<packet::kLittleEndian>(std::make_shared<std::vector<uint8_t>>(move(data_bytes)));
+    auto packet = packet::PacketView<packet::kLittleEndian>(
+        std::make_shared<std::vector<uint8_t>>(std::move(data_bytes)));
     auto iso = std::make_unique<IsoView>(IsoView::Create(packet));
-    module_.impl_->incoming_iso_buffer_.Enqueue(move(iso), module_.GetHandler());
+    module_.impl_->incoming_iso_buffer_.Enqueue(std::move(iso), module_.GetHandler());
   }
 
   HciLayer& module_;
@@ -449,8 +479,7 @@
 
 HciLayer::HciLayer() : impl_(nullptr), hal_callbacks_(nullptr) {}
 
-HciLayer::~HciLayer() {
-}
+HciLayer::~HciLayer() {}
 
 common::BidiQueueEnd<AclBuilder, AclView>* HciLayer::GetAclQueueEnd() {
   return impl_->acl_queue_.GetUpEnd();
@@ -466,12 +495,17 @@
 
 void HciLayer::EnqueueCommand(
     unique_ptr<CommandBuilder> command, ContextualOnceCallback<void(CommandCompleteView)> on_complete) {
-  CallOn(impl_, &impl::enqueue_command<CommandCompleteView>, move(command), move(on_complete));
+  CallOn(
+      impl_,
+      &impl::enqueue_command<CommandCompleteView>,
+      std::move(command),
+      std::move(on_complete));
 }
 
 void HciLayer::EnqueueCommand(
     unique_ptr<CommandBuilder> command, ContextualOnceCallback<void(CommandStatusView)> on_status) {
-  CallOn(impl_, &impl::enqueue_command<CommandStatusView>, move(command), move(on_status));
+  CallOn(
+      impl_, &impl::enqueue_command<CommandStatusView>, std::move(command), std::move(on_status));
 }
 
 void HciLayer::RegisterEventHandler(EventCode event, ContextualCallback<void(EventView)> handler) {
diff --git a/system/gd/hci/hci_packets.pdl b/system/gd/hci/hci_packets.pdl
index fa7373a..1960bf5 100644
--- a/system/gd/hci/hci_packets.pdl
+++ b/system/gd/hci/hci_packets.pdl
@@ -3832,6 +3832,10 @@
 }
 
 test LeSetExtendedAdvertisingEnable {
+  "\x39\x20\x06\x01\x01\x01\x00\x00\x00",
+}
+
+test LeSetExtendedAdvertisingDisable {
   "\x39\x20\x06\x00\x01\x01\x00\x00\x00",
 }
 
diff --git a/system/gd/packet/parser/packet_def.cc b/system/gd/packet/parser/packet_def.cc
index 3fe2218..d857eff 100644
--- a/system/gd/packet/parser/packet_def.cc
+++ b/system/gd/packet/parser/packet_def.cc
@@ -1406,8 +1406,8 @@
     }
     for (size_t i = 1; i < lineage.size(); i++) {
       s << "_ => {";
-      s << "println!(\"Couldn't parse " << util::CamelCaseToUnderScore(lineage[lineage.size() - i]->name_);
-      s << "{:02x?}\", " << util::CamelCaseToUnderScore(lineage[lineage.size() - i - 1]->name_) << "_packet); ";
+      s << "panic!(\"Couldn't parse " << util::CamelCaseToUnderScore(lineage[lineage.size() - i]->name_);
+      s << "\n {:#02x?}\", " << util::CamelCaseToUnderScore(lineage[lineage.size() - i - 1]->name_) << "_packet); ";
       s << "}}}";
     }
 
diff --git a/system/gd/packet/parser/parent_def.cc b/system/gd/packet/parser/parent_def.cc
index 0df4a5d..a72a9d7 100644
--- a/system/gd/packet/parser/parent_def.cc
+++ b/system/gd/packet/parser/parent_def.cc
@@ -613,6 +613,8 @@
       FixedScalarField::kFieldType,
   });
 
+  s << "if bytes.len() < " << this->GetSize(false).bytes() << " { return false; }";
+
   for (auto const& field : fields) {
     auto start_offset = GetOffsetForField(field->GetName(), false);
     auto end_offset = GetOffsetForField(field->GetName(), true);
diff --git a/system/gd/packet/parser/test/rust_test_packets.pdl b/system/gd/packet/parser/test/rust_test_packets.pdl
index d791979..38ee025 100644
--- a/system/gd/packet/parser/test/rust_test_packets.pdl
+++ b/system/gd/packet/parser/test/rust_test_packets.pdl
@@ -82,27 +82,27 @@
 }
 
 test AddRes {
-  "\x00\x04\x04\x01\x04\x04",
+  "\x02\x00",
 }
 
 test SubRes {
-  "\x01\x04\x04\x01\x04\x04",
+  "\x03\x00",
 }
 
 test AddCommand {
-  "\x02\x04\x04\x01\x04\x04",
+  "\x04\x00",
 }
 
 test SubCommand {
-  "\x03\x04\x04\x01\x04\x04",
+  "\x05\x00",
 }
 
 test AddErr {
-  "\x04\x04\x04\x01\x04\x04",
+  "\x00\x00",
 }
 
 test SubErr {
-  "\x05\x04\x04\x01\x04\x04",
+  "\x01\x00",
 }
 
 
@@ -148,17 +148,17 @@
 }
 
 test ChildOneTwo {
-  "\x01\x02\x03\x01",
+  "\x01\x02\x03\x01\x01\x02\x03",
 }
 
 test ChildThreeFour {
-  "\x03\x03\x03\x01",
+  "\x03\x04\x03\x01\x03\x04\x03",
 }
 
 test ChildThree {
-  "\x02\x01\x04\x01",
+  "\x01\x04\x03\x04\x03\x00\x02\x05",
 }
 
 test GrandChildThreeFive {
-  "\x01\x02\x03\x01",
+  "\x01\x04\x03\x04\x03\x00\x02\x05",
 }
diff --git a/system/gd/rust/common/BUILD.gn b/system/gd/rust/common/BUILD.gn
index 80d5a23..9a6e8ca 100644
--- a/system/gd/rust/common/BUILD.gn
+++ b/system/gd/rust/common/BUILD.gn
@@ -15,29 +15,32 @@
 
 import("//common-mk/cxxbridge.gni")
 
-rust_library("libbt_common") {
-  crate_name = "bt_common"
-
-  sources = [ "src/lib.rs" ]
-
-  configs = [
-    "//bt/system/gd/rust/shim:rust_libs",
-    "//bt/system/gd:rust_defaults",
+static_library("libbt_keystore_cc") {
+  complete_static_lib = true
+  sources = [ "keystore/fake_bt_keystore.cc" ]
+  deps = [
+    ":libbt_common_bridge_code"
   ]
+
+  configs += ["//bt/system/gd:gd_defaults"]
 }
 
-cxxbridge_cc("libbt_common_sys_prop_bridge_code") {
-  sources = [ "src/sys_prop.rs" ]
+config("rust_common_config") {
+  include_dirs = [ "//bt/system/gd/rust/common" ]
+}
 
+cxxbridge_header("libbt_common_bridge_header") {
+  sources = [ "src/bridge.rs" ]
+  all_dependent_configs = [ ":rust_common_config" ]
+  deps = [ ":cxxlibheader" ]
+}
+
+cxxbridge_cc("libbt_common_bridge_code") {
+  sources = [ "src/bridge.rs" ]
+  deps = [ ":libbt_common_bridge_header" ]
   configs = [ "//bt/system/gd:gd_defaults" ]
 }
 
-static_library("libbt_common_sys_prop_cxx") {
-  sources = [ "src/ffi/sys_props.cc" ]
-
-  include_dirs = [ "src/ffi" ]
-
-  deps = [ ":libbt_common_sys_prop_bridge_code" ]
-
-  configs += [ "//bt/system/gd:gd_defaults" ]
+cxxbridge_libheader("cxxlibheader") {
+  deps = []
 }
diff --git a/system/gd/rust/common/Cargo.toml b/system/gd/rust/common/Cargo.toml
index 22de79a..88b3918 100644
--- a/system/gd/rust/common/Cargo.toml
+++ b/system/gd/rust/common/Cargo.toml
@@ -21,6 +21,7 @@
 [dependencies]
 cxx = "*"
 env_logger = "*"
+futures = "0.3.13"
 grpcio = "*"
 lazy_static = "*"
 log = "*"
diff --git a/system/gd/rust/common/build.rs b/system/gd/rust/common/build.rs
new file mode 100644
index 0000000..73a5adb
--- /dev/null
+++ b/system/gd/rust/common/build.rs
@@ -0,0 +1,12 @@
+fn main() {
+    let target_dir = std::env::var_os("CARGO_TARGET_DIR").unwrap();
+
+    // Link the keystore static lib and make sure to use -lc++
+    println!("cargo:rustc-link-lib=static=bt_keystore_cc");
+    println!("cargo:rustc-link-search=native={}", target_dir.clone().into_string().unwrap());
+    println!("cargo:rustc-link-lib=c++");
+
+    // Re-run if static libs or this file changed.
+    println!("cargo:rerun-if-changed={}", target_dir.into_string().unwrap());
+    println!("cargo:rerun-if-changed=build.rs");
+}
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/main.rs b/system/gd/rust/linux/mgmt/src/bin/btmanagerd/main.rs
index f73629e..f37c614 100644
--- a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/main.rs
+++ b/system/gd/rust/linux/mgmt/src/bin/btmanagerd/main.rs
@@ -1,30 +1,21 @@
-mod bluetooth_manager;
-mod bluetooth_manager_dbus;
-mod config_util;
-mod dbus_arg;
-mod dbus_iface;
-mod powerd_suspend_manager;
-mod service_watcher;
-mod state_machine;
+// The manager binary (btmanagerd) is a fairly barebone bin file that depends on the manager_service
+// library which implements most of the logic. The code is separated in this way so that we can
+// apply certain linker flags (which is applied to the library but not the binary).
+// Please keep main.rs logic light and write the heavy logic in the manager_service library instead.
 
-use crate::bluetooth_manager::BluetoothManager;
-use crate::powerd_suspend_manager::PowerdSuspendManager;
 use dbus::channel::MatchingReceiver;
 use dbus::message::MatchRule;
 use dbus_crossroads::Crossroads;
 use dbus_projection::DisconnectWatcher;
 use dbus_tokio::connection;
 use log::LevelFilter;
+use manager_service::bluetooth_manager::{BluetoothManager, ManagerContext};
+use manager_service::powerd_suspend_manager::PowerdSuspendManager;
+use manager_service::{bluetooth_manager_dbus, config_util, state_machine};
 use std::sync::atomic::AtomicBool;
 use std::sync::{Arc, Mutex};
 use syslog::{BasicLogger, Facility, Formatter3164};
 
-#[derive(Clone)]
-struct ManagerContext {
-    proxy: state_machine::StateMachineProxy,
-    floss_enabled: Arc<AtomicBool>,
-}
-
 #[tokio::main]
 pub async fn main() -> Result<(), Box<dyn std::error::Error>> {
     let formatter = Formatter3164 {
@@ -62,10 +53,8 @@
 
     let context = state_machine::start_new_state_machine_context(invoker);
     let proxy = context.get_proxy();
-    let manager_context = ManagerContext {
-        proxy: proxy,
-        floss_enabled: Arc::new(AtomicBool::new(config_util::is_floss_enabled())),
-    };
+    let manager_context =
+        ManagerContext::new(proxy, Arc::new(AtomicBool::new(config_util::is_floss_enabled())));
 
     // The resource is a task that should be spawned onto a tokio compatible
     // reactor ASAP. If the resource ever finishes, you lost connection to D-Bus.
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/bluetooth_manager.rs b/system/gd/rust/linux/mgmt/src/bluetooth_manager.rs
similarity index 91%
rename from system/gd/rust/linux/mgmt/src/bin/btmanagerd/bluetooth_manager.rs
rename to system/gd/rust/linux/mgmt/src/bluetooth_manager.rs
index 706fb11..8de6cd0 100644
--- a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/bluetooth_manager.rs
+++ b/system/gd/rust/linux/mgmt/src/bluetooth_manager.rs
@@ -1,17 +1,29 @@
 use log::{error, info, warn};
 
-use manager_service::iface_bluetooth_manager::{
-    AdapterWithEnabled, IBluetoothManager, IBluetoothManagerCallback,
-};
-
 use std::collections::HashMap;
 use std::process::Command;
-use std::sync::atomic::Ordering;
+use std::sync::atomic::{AtomicBool, Ordering};
+use std::sync::Arc;
 
-use crate::{config_util, state_machine, ManagerContext};
+use crate::iface_bluetooth_manager::{
+    AdapterWithEnabled, IBluetoothManager, IBluetoothManagerCallback,
+};
+use crate::{config_util, state_machine};
 
 const BLUEZ_INIT_TARGET: &str = "bluetoothd";
 
+#[derive(Clone)]
+pub struct ManagerContext {
+    proxy: state_machine::StateMachineProxy,
+    floss_enabled: Arc<AtomicBool>,
+}
+
+impl ManagerContext {
+    pub fn new(proxy: state_machine::StateMachineProxy, floss_enabled: Arc<AtomicBool>) -> Self {
+        Self { proxy, floss_enabled }
+    }
+}
+
 /// Implementation of IBluetoothManager.
 pub struct BluetoothManager {
     manager_context: ManagerContext,
@@ -20,7 +32,7 @@
 }
 
 impl BluetoothManager {
-    pub(crate) fn new(manager_context: ManagerContext) -> BluetoothManager {
+    pub fn new(manager_context: ManagerContext) -> BluetoothManager {
         BluetoothManager {
             manager_context,
             callbacks: HashMap::new(),
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/bluetooth_manager_dbus.rs b/system/gd/rust/linux/mgmt/src/bluetooth_manager_dbus.rs
similarity index 97%
rename from system/gd/rust/linux/mgmt/src/bin/btmanagerd/bluetooth_manager_dbus.rs
rename to system/gd/rust/linux/mgmt/src/bluetooth_manager_dbus.rs
index 8ee912f..73b3c03 100644
--- a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/bluetooth_manager_dbus.rs
+++ b/system/gd/rust/linux/mgmt/src/bluetooth_manager_dbus.rs
@@ -4,11 +4,11 @@
 use dbus_projection::{dbus_generated, DisconnectWatcher};
 
 use btstack::RPCProxy;
-use manager_service::iface_bluetooth_manager::{
-    AdapterWithEnabled, IBluetoothManager, IBluetoothManagerCallback,
-};
 
 use crate::dbus_arg::{DBusArg, DBusArgError, RefArgToRust};
+use crate::iface_bluetooth_manager::{
+    AdapterWithEnabled, IBluetoothManager, IBluetoothManagerCallback,
+};
 
 #[dbus_propmap(AdapterWithEnabled)]
 pub struct AdapterWithEnabledDbus {
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/config_util.rs b/system/gd/rust/linux/mgmt/src/config_util.rs
similarity index 100%
rename from system/gd/rust/linux/mgmt/src/bin/btmanagerd/config_util.rs
rename to system/gd/rust/linux/mgmt/src/config_util.rs
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/dbus_arg.rs b/system/gd/rust/linux/mgmt/src/dbus_arg.rs
similarity index 100%
rename from system/gd/rust/linux/mgmt/src/bin/btmanagerd/dbus_arg.rs
rename to system/gd/rust/linux/mgmt/src/dbus_arg.rs
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/dbus_iface.rs b/system/gd/rust/linux/mgmt/src/dbus_iface.rs
similarity index 100%
rename from system/gd/rust/linux/mgmt/src/bin/btmanagerd/dbus_iface.rs
rename to system/gd/rust/linux/mgmt/src/dbus_iface.rs
diff --git a/system/gd/rust/linux/mgmt/src/lib.rs b/system/gd/rust/linux/mgmt/src/lib.rs
index 6a091f2..cd761ea 100644
--- a/system/gd/rust/linux/mgmt/src/lib.rs
+++ b/system/gd/rust/linux/mgmt/src/lib.rs
@@ -1,4 +1,12 @@
+pub mod bluetooth_manager;
+pub mod bluetooth_manager_dbus;
+pub mod config_util;
+pub mod dbus_arg;
+pub mod dbus_iface;
 pub mod iface_bluetooth_manager;
+pub mod powerd_suspend_manager;
+pub mod service_watcher;
+pub mod state_machine;
 
 // protoc-rust generates all modules and exports them in mod.rs
 // We have to include them all here to make them available for crate export.
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/powerd_suspend_manager.rs b/system/gd/rust/linux/mgmt/src/powerd_suspend_manager.rs
similarity index 96%
rename from system/gd/rust/linux/mgmt/src/bin/btmanagerd/powerd_suspend_manager.rs
rename to system/gd/rust/linux/mgmt/src/powerd_suspend_manager.rs
index 9f85703..fb9faae 100644
--- a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/powerd_suspend_manager.rs
+++ b/system/gd/rust/linux/mgmt/src/powerd_suspend_manager.rs
@@ -5,16 +5,16 @@
 use dbus::nonblock::SyncConnection;
 use dbus_crossroads::Crossroads;
 use dbus_projection::DisconnectWatcher;
-use manager_service::suspend::{
-    RegisterSuspendDelayReply, RegisterSuspendDelayRequest, SuspendDone, SuspendImminent,
-    SuspendImminent_Reason, SuspendReadinessInfo,
-};
 use protobuf::{CodedInputStream, CodedOutputStream, Message};
 use std::sync::{Arc, Mutex};
 use std::time::Duration;
 
 use crate::dbus_iface::{export_suspend_callback_dbus_obj, SuspendDBus};
 use crate::service_watcher::ServiceWatcher;
+use crate::suspend::{
+    RegisterSuspendDelayReply, RegisterSuspendDelayRequest, SuspendDone, SuspendImminent,
+    SuspendImminent_Reason, SuspendReadinessInfo,
+};
 
 const POWERD_SERVICE: &str = "org.chromium.PowerManager";
 const POWERD_INTERFACE: &str = "org.chromium.PowerManager";
@@ -118,20 +118,20 @@
         // Received when adapter is ready to suspend. Tell powerd that suspend is ready.
         log::debug!("Suspend ready, adapter suspend_id = {}", suspend_id);
 
-        if let Some(pending_suspend_imminent) =
-            &self.context.lock().unwrap().pending_suspend_imminent
         {
-            if let Some(powerd_session) = &self.context.lock().unwrap().powerd_session {
+            let context = self.context.lock().unwrap();
+
+            if let (Some(pending_suspend_imminent), Some(powerd_session)) =
+                (&context.pending_suspend_imminent, &context.powerd_session)
+            {
                 send_handle_suspend_readiness(
                     powerd_session.powerd_proxy.clone(),
                     powerd_session.delay_id,
                     pending_suspend_imminent.get_suspend_id(),
                 );
             } else {
-                log::warn!("Suspend is ready but there is no powerd session to report to");
+                log::warn!("Suspend ready but no SuspendImminent signal or powerd session");
             }
-        } else {
-            log::warn!("Suspend ready but no SuspendImminent signal");
         }
     }
 
@@ -395,9 +395,13 @@
         // still have pending SuspendImminent.
         log::debug!("powerd stopped, cleaning up");
 
-        match self.context.lock().unwrap().powerd_session {
-            None => log::warn!("powerd session does not exist, ignoring"),
-            Some(_) => self.context.lock().unwrap().powerd_session = None,
+        {
+            let mut context = self.context.lock().unwrap();
+
+            match context.powerd_session {
+                None => log::warn!("powerd session does not exist, ignoring"),
+                Some(_) => context.powerd_session = None,
+            }
         }
     }
 
@@ -468,10 +472,11 @@
         self.context.lock().unwrap().adapter_suspend_dbus =
             Some(SuspendDBus::new(conn.clone(), path));
 
+        let crossroads = self.context.lock().unwrap().dbus_crossroads.clone();
+
         if let Some(adapter_suspend_dbus) = &mut self.context.lock().unwrap().adapter_suspend_dbus {
             let suspend_cb_objpath: String =
                 format!("/org/chromium/bluetooth/Manager/suspend_callback");
-            let crossroads = self.context.lock().unwrap().dbus_crossroads.clone();
             adapter_suspend_dbus.register_callback(Box::new(SuspendCallback::new(
                 suspend_cb_objpath,
                 conn,
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/service_watcher.rs b/system/gd/rust/linux/mgmt/src/service_watcher.rs
similarity index 100%
rename from system/gd/rust/linux/mgmt/src/bin/btmanagerd/service_watcher.rs
rename to system/gd/rust/linux/mgmt/src/service_watcher.rs
diff --git a/system/gd/rust/linux/mgmt/src/bin/btmanagerd/state_machine.rs b/system/gd/rust/linux/mgmt/src/state_machine.rs
similarity index 100%
rename from system/gd/rust/linux/mgmt/src/bin/btmanagerd/state_machine.rs
rename to system/gd/rust/linux/mgmt/src/state_machine.rs
diff --git a/system/gd/rust/packets/test_lib.rs b/system/gd/rust/packets/test_lib.rs
index 960178c..db6fbe6 100644
--- a/system/gd/rust/packets/test_lib.rs
+++ b/system/gd/rust/packets/test_lib.rs
@@ -98,4 +98,11 @@
         let res = TestBodySizePacket::parse(&input);
         assert!(res.is_ok());
     }
+
+    #[test]
+    fn test_invalid_grand_child_three_five_size() {
+        let input = [0x1, 0x4, 0x3, 0x4, 0x3, 0x0, 0x2 /*, 0x5*/];
+        let res = GrandParentPacket::parse(&input);
+        assert!(res.is_err());
+    }
 }
diff --git a/system/include/hardware/bt_vc.h b/system/include/hardware/bt_vc.h
index 18fd764..090ff9c 100644
--- a/system/include/hardware/bt_vc.h
+++ b/system/include/hardware/bt_vc.h
@@ -86,6 +86,12 @@
   /** Set the volume */
   virtual void SetVolume(std::variant<RawAddress, int> addr_or_group_id,
                          uint8_t volume) = 0;
+  /** Mute the volume */
+  virtual void Mute(std::variant<RawAddress, int> addr_or_group_id) = 0;
+
+  /** Unmute the volume */
+  virtual void Unmute(std::variant<RawAddress, int> addr_or_group_id) = 0;
+
   virtual void GetExtAudioOutVolumeOffset(const RawAddress& address,
                                           uint8_t ext_output_id) = 0;
   virtual void SetExtAudioOutVolumeOffset(const RawAddress& address,
diff --git a/system/main/shim/acl.cc b/system/main/shim/acl.cc
index 21678c8..95fce55 100644
--- a/system/main/shim/acl.cc
+++ b/system/main/shim/acl.cc
@@ -653,7 +653,7 @@
       return;
     }
 
-    if (page_number != max_page_number)
+    if (max_page_number != 0 && page_number != max_page_number)
       connection_->ReadRemoteExtendedFeatures(page_number + 1);
   }
 
diff --git a/system/main/test/main_shim_test.cc b/system/main/test/main_shim_test.cc
index a701ea6..5de3b7a 100644
--- a/system/main/test/main_shim_test.cc
+++ b/system/main/test/main_shim_test.cc
@@ -128,6 +128,10 @@
   mock_connection_le_on_disconnected_promise.set_value(handle);
 }
 
+void mock_link_classic_on_read_remote_extended_features_complete(
+    uint16_t handle, uint8_t current_page_number, uint8_t max_page_number,
+    uint64_t features) {}
+
 const shim::legacy::acl_interface_t GetMockAclInterface() {
   shim::legacy::acl_interface_t acl_interface{
       .on_send_data_upwards = mock_on_send_data_upwards,
@@ -165,7 +169,8 @@
       .link.classic.on_read_link_quality_complete = nullptr,
       .link.classic.on_read_link_supervision_timeout_complete = nullptr,
       .link.classic.on_read_remote_version_information_complete = nullptr,
-      .link.classic.on_read_remote_extended_features_complete = nullptr,
+      .link.classic.on_read_remote_extended_features_complete =
+          mock_link_classic_on_read_remote_extended_features_complete,
       .link.classic.on_read_rssi_complete = nullptr,
       .link.classic.on_read_transmit_power_level_complete = nullptr,
       .link.classic.on_role_change = nullptr,
@@ -235,6 +240,15 @@
   bool ReadRemoteVersionInformation() override { return true; }
   bool ReadRemoteSupportedFeatures() override { return true; }
 
+  std::function<void(uint8_t)> read_remote_extended_features_function_{};
+
+  bool ReadRemoteExtendedFeatures(uint8_t page_number) override {
+    if (read_remote_extended_features_function_) {
+      read_remote_extended_features_function_(page_number);
+    }
+    return true;
+  }
+
   bool Disconnect(hci::DisconnectReason reason) override {
     disconnect_cnt_++;
     disconnect_promise_.set_value(handle_);
@@ -280,6 +294,7 @@
   MockDeQueue<packet::PacketView<hci::kLittleEndian>> rx_;
 
   bool ReadRemoteVersionInformation() override { return true; }
+  bool LeReadRemoteFeatures() override { return true; }
 
   void Disconnect(hci::DisconnectReason reason) override {
     disconnect_cnt_++;
@@ -379,6 +394,65 @@
   }
 };
 
+class MainShimTestWithClassicConnection : public MainShimTest {
+ protected:
+  void SetUp() override {
+    MainShimTest::SetUp();
+    hci::Address address({0x11, 0x22, 0x33, 0x44, 0x55, 0x66});
+
+    acl_ = MakeAcl();
+
+    // Create connection
+    EXPECT_CALL(*test::mock_acl_manager_, CreateConnection(_)).Times(1);
+    acl_->CreateClassicConnection(address);
+
+    // Respond with a mock connection created
+    auto connection = std::make_unique<MockClassicAclConnection>(address, 123);
+    ASSERT_EQ(123, connection->GetHandle());
+    ASSERT_EQ(hci::Address({0x11, 0x22, 0x33, 0x44, 0x55, 0x66}),
+              connection->GetAddress());
+    raw_connection_ = connection.get();
+
+    acl_->OnConnectSuccess(std::move(connection));
+    ASSERT_EQ(nullptr, connection);
+    ASSERT_NE(nullptr, raw_connection_->callbacks_);
+  }
+
+  void TearDown() override {
+    // Specify local disconnect request
+    auto tx_disconnect_future =
+        raw_connection_->disconnect_promise_.get_future();
+    acl_->DisconnectClassic(123, HCI_SUCCESS, {});
+
+    // Wait for disconnect to be received
+    uint16_t result = tx_disconnect_future.get();
+    ASSERT_EQ(123, result);
+
+    // Now emulate the remote disconnect response
+    auto handle_promise = std::promise<uint16_t>();
+    auto rx_disconnect_future = handle_promise.get_future();
+    mock_function_handle_promise_map
+        ["mock_connection_classic_on_disconnected"] = std::move(handle_promise);
+    raw_connection_->callbacks_->OnDisconnection(hci::ErrorCode::SUCCESS);
+
+    result = rx_disconnect_future.get();
+    ASSERT_EQ(123, result);
+
+    // *Our* task completing indicates reactor is done
+    std::promise<void> done;
+    auto future = done.get_future();
+    handler_->Call([](std::promise<void> done) { done.set_value(); },
+                   std::move(done));
+    future.wait();
+
+    acl_.reset();
+
+    MainShimTest::TearDown();
+  }
+  std::unique_ptr<shim::legacy::Acl> acl_;
+  MockClassicAclConnection* raw_connection_{nullptr};
+};
+
 TEST_F(MainShimTest, Nop) {}
 
 TEST_F(MainShimTest, Acl_Lifecycle) {
@@ -512,7 +586,7 @@
   void OnBatchScanThresholdCrossed(int client_if) override {}
 };
 
-TEST_F(MainShimTest, BleScannerInterfaceImpl_OnScanResult) {
+TEST_F(MainShimTest, DISABLED_BleScannerInterfaceImpl_OnScanResult) {
   auto* ble = static_cast<bluetooth::shim::BleScannerInterfaceImpl*>(
       bluetooth::shim::get_ble_scanner_instance());
 
@@ -554,7 +628,7 @@
     nullptr,
 };
 
-TEST_F(MainShimTest, LeShimAclConnection_local_disconnect) {
+TEST_F(MainShimTest, DISABLED_LeShimAclConnection_local_disconnect) {
   bluetooth::common::InitFlags::Load(test_flags);
   auto acl = MakeAcl();
   EXPECT_CALL(*test::mock_acl_manager_, CreateLeConnection(_, _)).Times(1);
@@ -579,6 +653,8 @@
                                                           remote_address, role);
   auto raw_connection = connection.get();
   acl->OnLeConnectSuccess(remote_address, std::move(connection));
+  ASSERT_EQ(nullptr, connection);
+  ASSERT_NE(nullptr, raw_connection->callbacks_);
 
   // Initiate local LE disconnect
   mock_connection_le_on_disconnected_promise = std::promise<uint16_t>();
@@ -595,3 +671,50 @@
 
   ASSERT_EQ(0x1234, disconnect_future.get());
 }
+
+TEST_F(MainShimTestWithClassicConnection, nop) {}
+
+TEST_F(MainShimTestWithClassicConnection, read_extended_feature) {
+  int read_remote_extended_feature_call_count = 0;
+  raw_connection_->read_remote_extended_features_function_ =
+      [&read_remote_extended_feature_call_count](uint8_t page_number) {
+        read_remote_extended_feature_call_count++;
+      };
+
+  // Handle typical case
+  {
+    read_remote_extended_feature_call_count = 0;
+    const uint8_t max_page = 3;
+    raw_connection_->callbacks_->OnReadRemoteExtendedFeaturesComplete(
+        1, max_page, 0xabcdef9876543210);
+    raw_connection_->callbacks_->OnReadRemoteExtendedFeaturesComplete(
+        2, max_page, 0xbcdef9876543210a);
+    raw_connection_->callbacks_->OnReadRemoteExtendedFeaturesComplete(
+        3, max_page, 0xcdef9876543210ab);
+    ASSERT_EQ(static_cast<int>(max_page) - 1,
+              read_remote_extended_feature_call_count);
+  }
+
+  // Handle extreme case
+  {
+    read_remote_extended_feature_call_count = 0;
+    const uint8_t max_page = 255;
+    for (int page = 1; page < static_cast<int>(max_page) + 1; page++) {
+      raw_connection_->callbacks_->OnReadRemoteExtendedFeaturesComplete(
+          static_cast<uint8_t>(page), max_page, 0xabcdef9876543210);
+    }
+    ASSERT_EQ(static_cast<int>(max_page - 1),
+              read_remote_extended_feature_call_count);
+  }
+
+  // Handle case where device returns max page of zero
+  {
+    read_remote_extended_feature_call_count = 0;
+    const uint8_t max_page = 0;
+    raw_connection_->callbacks_->OnReadRemoteExtendedFeaturesComplete(
+        1, max_page, 0xabcdef9876543210);
+    ASSERT_EQ(0, read_remote_extended_feature_call_count);
+  }
+
+  raw_connection_->read_remote_extended_features_function_ = {};
+}
diff --git a/system/stack/Android.bp b/system/stack/Android.bp
index a244f14..f132f28 100644
--- a/system/stack/Android.bp
+++ b/system/stack/Android.bp
@@ -1142,3 +1142,70 @@
         },
     },
 }
+
+cc_test {
+    name: "net_test_stack_acl",
+    test_suites: ["device-tests"],
+    host_supported: true,
+    defaults: ["fluoride_defaults"],
+    local_include_dirs: [
+        "include",
+        "test/common",
+    ],
+    include_dirs: [
+        "packages/modules/Bluetooth/system",
+        "packages/modules/Bluetooth/system/gd",
+        "packages/modules/Bluetooth/system/utils/include",
+    ],
+    generated_headers: [
+        "BluetoothGeneratedDumpsysDataSchema_h",
+        "BluetoothGeneratedPackets_h",
+    ],
+    srcs: [
+        ":OsiCompatSources",
+        ":TestCommonMainHandler",
+        ":TestCommonMockFunctions",
+        ":TestCommonStackConfig",
+        ":TestMockBta",
+        ":TestMockBtif",
+        ":TestMockDevice",
+        ":TestMockHci",
+        ":TestMockLegacyHciCommands",
+        ":TestMockLegacyHciInterface",
+        ":TestMockMainShim",
+        ":TestMockStackBtm",
+        ":TestMockStackBtu",
+        ":TestMockStackCryptotoolbox",
+        ":TestMockStackGatt",
+        ":TestMockStackHcic",
+        ":TestMockStackL2cap",
+        ":TestMockStackMetrics",
+        ":TestMockStackSdp",
+        ":TestMockStackSmp",
+        "acl/*.cc",
+        "test/stack_acl_test.cc",
+    ],
+    static_libs: [
+        "libbt-common",
+        "libbt-protos-lite",
+        "libgmock",
+        "liblog",
+        "libosi",
+    ],
+    shared_libs: [
+        "libbinder_ndk",
+        "libcrypto",
+        "libflatbuffers-cpp",
+        "libprotobuf-cpp-lite",
+    ],
+    sanitize: {
+        address: true,
+        all_undefined: true,
+        cfi: true,
+        integer_overflow: true,
+        scs: true,
+        diag: {
+            undefined : true
+        },
+    },
+}
diff --git a/system/stack/acl/btm_acl.cc b/system/stack/acl/btm_acl.cc
index df6ec16..5632151 100644
--- a/system/stack/acl/btm_acl.cc
+++ b/system/stack/acl/btm_acl.cc
@@ -2761,21 +2761,21 @@
 }
 
 bool acl_create_le_connection_with_id(uint8_t id, const RawAddress& bd_addr) {
-    tBLE_BD_ADDR address_with_type{
-        .bda = bd_addr,
-        .type = BLE_ADDR_RANDOM,
-    };
-    gatt_find_in_device_record(bd_addr, &address_with_type);
-    LOG_DEBUG("Creating le direct connection to:%s",
-              PRIVATE_ADDRESS(address_with_type));
+  tBLE_BD_ADDR address_with_type{
+      .bda = bd_addr,
+      .type = BLE_ADDR_PUBLIC,
+  };
+  gatt_find_in_device_record(bd_addr, &address_with_type);
+  LOG_DEBUG("Creating le direct connection to:%s",
+            PRIVATE_ADDRESS(address_with_type));
 
-    if (address_with_type.type == BLE_ADDR_ANONYMOUS) {
-      LOG_WARN(
-          "Creating le direct connection to:%s, address type 'anonymous' is "
-          "invalid",
-          PRIVATE_ADDRESS(address_with_type));
-      return false;
-    }
+  if (address_with_type.type == BLE_ADDR_ANONYMOUS) {
+    LOG_WARN(
+        "Creating le direct connection to:%s, address type 'anonymous' is "
+        "invalid",
+        PRIVATE_ADDRESS(address_with_type));
+    return false;
+  }
 
     bluetooth::shim::ACL_AcceptLeConnectionFrom(address_with_type,
                                                 /* is_direct */ true);
@@ -2897,7 +2897,7 @@
       bd_features_text(p_acl->peer_lmp_feature_pages[current_page_number])
           .c_str());
 
-  if (max_page_number == current_page_number) {
+  if (max_page_number == 0 || max_page_number == current_page_number) {
     NotifyAclFeaturesReadComplete(*p_acl, max_page_number);
   }
 }
diff --git a/system/stack/btm/security_device_record.h b/system/stack/btm/security_device_record.h
index 360c7fb..67d5b52 100644
--- a/system/stack/btm/security_device_record.h
+++ b/system/stack/btm/security_device_record.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <base/logging.h>
 #include <base/strings/stringprintf.h>
 #include <string.h>
 
@@ -92,7 +93,7 @@
   uint32_t local_counter; /* local sign counter for sending signed write cmd*/
 } tBTM_SEC_BLE_KEYS;
 
-typedef struct {
+struct tBTM_SEC_BLE {
   RawAddress pseudo_addr; /* LE pseudo address of the device if different from
                           device address  */
  private:
@@ -101,7 +102,12 @@
  public:
   tBLE_ADDR_TYPE AddressType() const { return ble_addr_type_; }
   void SetAddressType(tBLE_ADDR_TYPE ble_addr_type) {
-    if (is_ble_addr_type_known(ble_addr_type)) ble_addr_type_ = ble_addr_type;
+    if (is_ble_addr_type_known(ble_addr_type)) {
+      ble_addr_type_ = ble_addr_type;
+    } else {
+      LOG(ERROR) << "Please don't store illegal addresses into security record:"
+                 << AddressTypeText(ble_addr_type);
+    }
   }
 
   tBLE_BD_ADDR identity_address_with_type;
@@ -121,7 +127,8 @@
 
   tBTM_LE_KEY_TYPE key_type; /* bit mask of valid key types in record */
   tBTM_SEC_BLE_KEYS keys;    /* LE device security info in peripheral rode */
-} tBTM_SEC_BLE;
+};
+typedef struct tBTM_SEC_BLE tBTM_SEC_BLE;
 
 enum : uint16_t {
   BTM_SEC_AUTHENTICATED = 0x0002,
diff --git a/system/stack/test/stack_acl_test.cc b/system/stack/test/stack_acl_test.cc
new file mode 100644
index 0000000..6b741e2
--- /dev/null
+++ b/system/stack/test/stack_acl_test.cc
@@ -0,0 +1,129 @@
+/*
+ * Copyright 2022 The Android Open Source Project
+ *
+ * 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.
+ */
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <cstdint>
+#include <deque>
+
+#include "common/init_flags.h"
+#include "osi/include/log.h"
+#include "stack/acl/acl.h"
+#include "stack/btm/btm_int_types.h"
+#include "stack/btm/security_device_record.h"
+#include "stack/include/acl_api.h"
+#include "stack/include/acl_hci_link_interface.h"
+#include "stack/include/hci_error_code.h"
+#include "test/common/mock_functions.h"
+#include "test/mock/mock_main_shim_acl_api.h"
+#include "types/ble_address_with_type.h"
+#include "types/hci_role.h"
+#include "types/raw_address.h"
+
+tBTM_CB btm_cb;
+
+void LogMsg(uint32_t trace_set_mask, const char* fmt_str, ...) {}
+
+namespace {
+const char* test_flags[] = {
+    "INIT_logging_debug_enabled_for_all=true",
+    nullptr,
+};
+
+const RawAddress kRawAddress = RawAddress({0x11, 0x22, 0x33, 0x44, 0x55, 0x66});
+}  // namespace
+
+namespace bluetooth {
+namespace testing {
+
+std::set<const RawAddress> copy_of_connected_with_both_public_and_random_set();
+
+}  // namespace testing
+}  // namespace bluetooth
+
+void BTM_update_version_info(const RawAddress& bd_addr,
+                             const remote_version_info& remote_version_info) {}
+
+void btm_sec_role_changed(tHCI_STATUS hci_status, const RawAddress& bd_addr,
+                          tHCI_ROLE new_role) {}
+
+class StackAclTest : public testing::Test {
+ protected:
+  void SetUp() override {
+    mock_function_count_map.clear();
+    bluetooth::common::InitFlags::Load(test_flags);
+  }
+  void TearDown() override {}
+
+  tBTM_SEC_DEV_REC device_record_;
+};
+
+TEST_F(StackAclTest, nop) {}
+
+TEST_F(StackAclTest, acl_process_extended_features) {
+  const uint16_t hci_handle = 0x123;
+  const tBT_TRANSPORT transport = BT_TRANSPORT_LE;
+  const tHCI_ROLE link_role = HCI_ROLE_CENTRAL;
+
+  btm_acl_created(kRawAddress, hci_handle, link_role, transport);
+  tACL_CONN* p_acl = btm_acl_for_bda(kRawAddress, transport);
+  ASSERT_NE(nullptr, p_acl);
+
+  // Handle typical case
+  {
+    const uint8_t max_page = 3;
+    memset((void*)p_acl->peer_lmp_feature_valid, 0,
+           HCI_EXT_FEATURES_PAGE_MAX + 1);
+    acl_process_extended_features(hci_handle, 1, max_page, 0xf123456789abcde);
+    acl_process_extended_features(hci_handle, 2, max_page, 0xef123456789abcd);
+    acl_process_extended_features(hci_handle, 3, max_page, 0xdef123456789abc);
+
+    /* page 0 is the standard feature set */
+    ASSERT_FALSE(p_acl->peer_lmp_feature_valid[0]);
+    ASSERT_TRUE(p_acl->peer_lmp_feature_valid[1]);
+    ASSERT_TRUE(p_acl->peer_lmp_feature_valid[2]);
+    ASSERT_TRUE(p_acl->peer_lmp_feature_valid[3]);
+  }
+
+  // Handle extreme case
+  {
+    const uint8_t max_page = 255;
+    memset((void*)p_acl->peer_lmp_feature_valid, 0,
+           HCI_EXT_FEATURES_PAGE_MAX + 1);
+    for (int i = 1; i < HCI_EXT_FEATURES_PAGE_MAX + 1; i++) {
+      acl_process_extended_features(hci_handle, static_cast<uint8_t>(i),
+                                    max_page, 0x123456789abcdef);
+    }
+    /* page 0 is the standard feature set */
+    ASSERT_FALSE(p_acl->peer_lmp_feature_valid[0]);
+    ASSERT_TRUE(p_acl->peer_lmp_feature_valid[1]);
+    ASSERT_TRUE(p_acl->peer_lmp_feature_valid[2]);
+    ASSERT_TRUE(p_acl->peer_lmp_feature_valid[3]);
+  }
+
+  // Handle case where device returns max page of zero
+  {
+    memset((void*)p_acl->peer_lmp_feature_valid, 0,
+           HCI_EXT_FEATURES_PAGE_MAX + 1);
+    acl_process_extended_features(hci_handle, 1, 0, 0xdef123456789abc);
+    ASSERT_FALSE(p_acl->peer_lmp_feature_valid[0]);
+    ASSERT_TRUE(p_acl->peer_lmp_feature_valid[1]);
+    ASSERT_FALSE(p_acl->peer_lmp_feature_valid[2]);
+    ASSERT_FALSE(p_acl->peer_lmp_feature_valid[3]);
+  }
+
+  btm_acl_removed(hci_handle);
+}
diff --git a/system/test/mock/mock_main_shim_acl_api.h b/system/test/mock/mock_main_shim_acl_api.h
new file mode 100644
index 0000000..cb08047
--- /dev/null
+++ b/system/test/mock/mock_main_shim_acl_api.h
@@ -0,0 +1,227 @@
+/*
+ * Copyright 2022 The Android Open Source Project
+ *
+ * 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.
+ */
+#pragma once
+
+/*
+ * Generated mock file from original source file
+ *   Functions generated:14
+ *
+ *  mockcify.pl ver 0.5.0
+ */
+
+#include <cstdint>
+#include <functional>
+#include <map>
+#include <string>
+
+extern std::map<std::string, int> mock_function_count_map;
+
+// Original included files, if any
+// NOTE: Since this is a mock file with mock definitions some number of
+//       include files may not be required.  The include-what-you-use
+//       still applies, but crafting proper inclusion is out of scope
+//       for this effort.  This compilation unit may compile as-is, or
+//       may need attention to prune from (or add to ) the inclusion set.
+#include <cstddef>
+#include <cstdint>
+#include <future>
+
+#include "gd/hci/acl_manager.h"
+#include "main/shim/acl_api.h"
+#include "main/shim/dumpsys.h"
+#include "main/shim/helpers.h"
+#include "main/shim/stack.h"
+#include "osi/include/allocator.h"
+#include "stack/include/bt_hdr.h"
+#include "types/ble_address_with_type.h"
+#include "types/raw_address.h"
+
+// Original usings
+
+// Mocked compile conditionals, if any
+
+namespace test {
+namespace mock {
+namespace main_shim_acl_api {
+
+// Shared state between mocked functions and tests
+// Name: ACL_AcceptLeConnectionFrom
+// Params: const tBLE_BD_ADDR& legacy_address_with_type, bool is_direct
+// Return: bool
+struct ACL_AcceptLeConnectionFrom {
+  static bool return_value;
+  std::function<bool(const tBLE_BD_ADDR& legacy_address_with_type,
+                     bool is_direct)>
+      body{[](const tBLE_BD_ADDR& legacy_address_with_type, bool is_direct) {
+        return return_value;
+      }};
+  bool operator()(const tBLE_BD_ADDR& legacy_address_with_type,
+                  bool is_direct) {
+    return body(legacy_address_with_type, is_direct);
+  };
+};
+extern struct ACL_AcceptLeConnectionFrom ACL_AcceptLeConnectionFrom;
+
+// Name: ACL_AddToAddressResolution
+// Params: const tBLE_BD_ADDR& legacy_address_with_type, const Octet16&
+// peer_irk, const Octet16& local_irk Return: void
+struct ACL_AddToAddressResolution {
+  std::function<void(const tBLE_BD_ADDR& legacy_address_with_type,
+                     const Octet16& peer_irk, const Octet16& local_irk)>
+      body{[](const tBLE_BD_ADDR& legacy_address_with_type,
+              const Octet16& peer_irk, const Octet16& local_irk) {}};
+  void operator()(const tBLE_BD_ADDR& legacy_address_with_type,
+                  const Octet16& peer_irk, const Octet16& local_irk) {
+    body(legacy_address_with_type, peer_irk, local_irk);
+  };
+};
+extern struct ACL_AddToAddressResolution ACL_AddToAddressResolution;
+
+// Name: ACL_CancelClassicConnection
+// Params: const RawAddress& raw_address
+// Return: void
+struct ACL_CancelClassicConnection {
+  std::function<void(const RawAddress& raw_address)> body{
+      [](const RawAddress& raw_address) {}};
+  void operator()(const RawAddress& raw_address) { body(raw_address); };
+};
+extern struct ACL_CancelClassicConnection ACL_CancelClassicConnection;
+
+// Name: ACL_ClearAddressResolution
+// Params:
+// Return: void
+struct ACL_ClearAddressResolution {
+  std::function<void()> body{[]() {}};
+  void operator()() { body(); };
+};
+extern struct ACL_ClearAddressResolution ACL_ClearAddressResolution;
+
+// Name: ACL_ClearFilterAcceptList
+// Params:
+// Return: void
+struct ACL_ClearFilterAcceptList {
+  std::function<void()> body{[]() {}};
+  void operator()() { body(); };
+};
+extern struct ACL_ClearFilterAcceptList ACL_ClearFilterAcceptList;
+
+// Name: ACL_ConfigureLePrivacy
+// Params: bool is_le_privacy_enabled
+// Return: void
+struct ACL_ConfigureLePrivacy {
+  std::function<void(bool is_le_privacy_enabled)> body{
+      [](bool is_le_privacy_enabled) {}};
+  void operator()(bool is_le_privacy_enabled) { body(is_le_privacy_enabled); };
+};
+extern struct ACL_ConfigureLePrivacy ACL_ConfigureLePrivacy;
+
+// Name: ACL_CreateClassicConnection
+// Params: const RawAddress& raw_address
+// Return: void
+struct ACL_CreateClassicConnection {
+  std::function<void(const RawAddress& raw_address)> body{
+      [](const RawAddress& raw_address) {}};
+  void operator()(const RawAddress& raw_address) { body(raw_address); };
+};
+extern struct ACL_CreateClassicConnection ACL_CreateClassicConnection;
+
+// Name: ACL_Disconnect
+// Params: uint16_t handle, bool is_classic, tHCI_STATUS reason, std::string
+// comment Return: void
+struct ACL_Disconnect {
+  std::function<void(uint16_t handle, bool is_classic, tHCI_STATUS reason,
+                     std::string comment)>
+      body{[](uint16_t handle, bool is_classic, tHCI_STATUS reason,
+              std::string comment) {}};
+  void operator()(uint16_t handle, bool is_classic, tHCI_STATUS reason,
+                  std::string comment) {
+    body(handle, is_classic, reason, comment);
+  };
+};
+extern struct ACL_Disconnect ACL_Disconnect;
+
+// Name: ACL_IgnoreAllLeConnections
+// Params:
+// Return: void
+struct ACL_IgnoreAllLeConnections {
+  std::function<void()> body{[]() {}};
+  void operator()() { body(); };
+};
+extern struct ACL_IgnoreAllLeConnections ACL_IgnoreAllLeConnections;
+
+// Name: ACL_IgnoreLeConnectionFrom
+// Params: const tBLE_BD_ADDR& legacy_address_with_type
+// Return: void
+struct ACL_IgnoreLeConnectionFrom {
+  std::function<void(const tBLE_BD_ADDR& legacy_address_with_type)> body{
+      [](const tBLE_BD_ADDR& legacy_address_with_type) {}};
+  void operator()(const tBLE_BD_ADDR& legacy_address_with_type) {
+    body(legacy_address_with_type);
+  };
+};
+extern struct ACL_IgnoreLeConnectionFrom ACL_IgnoreLeConnectionFrom;
+
+// Name: ACL_ReadConnectionAddress
+// Params: const RawAddress& pseudo_addr, RawAddress& conn_addr, tBLE_ADDR_TYPE*
+// p_addr_type Return: void
+struct ACL_ReadConnectionAddress {
+  std::function<void(const RawAddress& pseudo_addr, RawAddress& conn_addr,
+                     tBLE_ADDR_TYPE* p_addr_type)>
+      body{[](const RawAddress& pseudo_addr, RawAddress& conn_addr,
+              tBLE_ADDR_TYPE* p_addr_type) {}};
+  void operator()(const RawAddress& pseudo_addr, RawAddress& conn_addr,
+                  tBLE_ADDR_TYPE* p_addr_type) {
+    body(pseudo_addr, conn_addr, p_addr_type);
+  };
+};
+extern struct ACL_ReadConnectionAddress ACL_ReadConnectionAddress;
+
+// Name: ACL_RemoveFromAddressResolution
+// Params: const tBLE_BD_ADDR& legacy_address_with_type
+// Return: void
+struct ACL_RemoveFromAddressResolution {
+  std::function<void(const tBLE_BD_ADDR& legacy_address_with_type)> body{
+      [](const tBLE_BD_ADDR& legacy_address_with_type) {}};
+  void operator()(const tBLE_BD_ADDR& legacy_address_with_type) {
+    body(legacy_address_with_type);
+  };
+};
+extern struct ACL_RemoveFromAddressResolution ACL_RemoveFromAddressResolution;
+
+// Name: ACL_Shutdown
+// Params:
+// Return: void
+struct ACL_Shutdown {
+  std::function<void()> body{[]() {}};
+  void operator()() { body(); };
+};
+extern struct ACL_Shutdown ACL_Shutdown;
+
+// Name: ACL_WriteData
+// Params: uint16_t handle, BT_HDR* p_buf
+// Return: void
+struct ACL_WriteData {
+  std::function<void(uint16_t handle, BT_HDR* p_buf)> body{
+      [](uint16_t handle, BT_HDR* p_buf) {}};
+  void operator()(uint16_t handle, BT_HDR* p_buf) { body(handle, p_buf); };
+};
+extern struct ACL_WriteData ACL_WriteData;
+
+}  // namespace main_shim_acl_api
+}  // namespace mock
+}  // namespace test
+
+// END mockcify generation
diff --git a/system/types/ble_address_with_type.h b/system/types/ble_address_with_type.h
index ecd62a8..38a52cc 100644
--- a/system/types/ble_address_with_type.h
+++ b/system/types/ble_address_with_type.h
@@ -119,5 +119,24 @@
   std::string ToString() const {
     return std::string(bda.ToString() + "[" + AddressTypeText(type) + "]");
   }
+  bool operator==(const tBLE_BD_ADDR rhs) const {
+    return rhs.type == type && rhs.bda == bda;
+  }
+  bool operator!=(const tBLE_BD_ADDR rhs) const { return !(*this == rhs); }
 };
+
+template <>
+struct std::hash<tBLE_BD_ADDR> {
+  std::size_t operator()(const tBLE_BD_ADDR& val) const {
+    static_assert(sizeof(uint64_t) >=
+                  (RawAddress::kLength + sizeof(tBLE_ADDR_TYPE)));
+    uint64_t int_addr = 0;
+    memcpy(reinterpret_cast<uint8_t*>(&int_addr), val.bda.address,
+           RawAddress::kLength);
+    memcpy(reinterpret_cast<uint8_t*>(&int_addr) + RawAddress::kLength,
+           (const void*)&val.type, sizeof(tBLE_ADDR_TYPE));
+    return std::hash<uint64_t>{}(int_addr);
+  }
+};
+
 #endif
diff --git a/tools/pdl/Android.bp b/tools/pdl/Android.bp
index b1cbcd4..e4d05fd 100644
--- a/tools/pdl/Android.bp
+++ b/tools/pdl/Android.bp
@@ -1,4 +1,3 @@
-
 package {
     // See: http://go/android-license-faq
     // A large-scale-change added 'default_applicable_licenses' to import
@@ -17,6 +16,11 @@
         "libserde_json",
         "libstructopt",
         "libcodespan_reporting",
+        "libquote",
+        "libsyn",
+        "libproc_macro2",
+        "libanyhow",
+        "libtempfile",
     ],
     proc_macros: [
         "libpest_derive",
@@ -32,4 +36,8 @@
     name: "pdl_inline_tests",
     defaults: ["pdl_defaults"],
     test_suites: ["general-tests"],
+    data: [
+        "rustfmt",
+        "rustfmt.toml",
+    ],
 }
diff --git a/system/gd/packet/parser/doc/reference.md b/tools/pdl/doc/reference.md
similarity index 100%
rename from system/gd/packet/parser/doc/reference.md
rename to tools/pdl/doc/reference.md
diff --git a/tools/pdl/rustfmt b/tools/pdl/rustfmt
new file mode 120000
index 0000000..a390621
--- /dev/null
+++ b/tools/pdl/rustfmt
@@ -0,0 +1 @@
+../../../../../prebuilts/rust/linux-x86/stable/rustfmt
\ No newline at end of file
diff --git a/tools/pdl/rustfmt.toml b/tools/pdl/rustfmt.toml
new file mode 120000
index 0000000..760eb84
--- /dev/null
+++ b/tools/pdl/rustfmt.toml
@@ -0,0 +1 @@
+../../rustfmt.toml
\ No newline at end of file
diff --git a/tools/pdl/src/generator.rs b/tools/pdl/src/generator.rs
new file mode 100644
index 0000000..a6de22b
--- /dev/null
+++ b/tools/pdl/src/generator.rs
@@ -0,0 +1,640 @@
+use crate::ast;
+use anyhow::{anyhow, bail, Context, Result};
+use quote::{format_ident, quote};
+use std::collections::HashMap;
+use std::path::Path;
+use syn::parse_quote;
+
+/// Generate a block of code.
+///
+/// Like `quote!`, but the code block will be followed by an empty
+/// line of code. This makes the generated code more readable.
+macro_rules! quote_block {
+    ($($tt:tt)*) => {
+        format!("{}\n\n", quote!($($tt)*))
+    }
+}
+
+/// Generate the file preamble.
+fn generate_preamble(path: &Path) -> Result<String> {
+    let mut code = String::new();
+    let filename = path
+        .file_name()
+        .and_then(|path| path.to_str())
+        .ok_or_else(|| anyhow!("could not find filename in {:?}", path))?;
+    code.push_str(&format!("// @generated rust packets from {filename}\n\n"));
+
+    code.push_str(&quote_block! {
+        use bytes::{BufMut, Bytes, BytesMut};
+        use num_derive::{FromPrimitive, ToPrimitive};
+        use num_traits::{FromPrimitive, ToPrimitive};
+        use std::convert::{TryFrom, TryInto};
+        use std::fmt;
+        use std::sync::Arc;
+        use thiserror::Error;
+    });
+
+    code.push_str(&quote_block! {
+        type Result<T> = std::result::Result<T, Error>;
+    });
+
+    code.push_str(&quote_block! {
+        #[derive(Debug, Error)]
+        pub enum Error {
+            #[error("Packet parsing failed")]
+            InvalidPacketError,
+            #[error("{field} was {value:x}, which is not known")]
+            ConstraintOutOfBounds { field: String, value: u64 },
+            #[error("when parsing {obj}.{field} needed length of {wanted} but got {got}")]
+            InvalidLengthError {
+                obj: String,
+                field: String,
+                wanted: usize,
+                got: usize,
+            },
+            #[error("Due to size restrictions a struct could not be parsed.")]
+            ImpossibleStructError,
+            #[error("when parsing field {obj}.{field}, {value} is not a valid {type_} value")]
+            InvalidEnumValueError {
+                obj: String,
+                field: String,
+                value: u64,
+                type_: String,
+            },
+        }
+    });
+
+    code.push_str(&quote_block! {
+        #[derive(Debug, Error)]
+        #[error("{0}")]
+        pub struct TryFromError(&'static str);
+    });
+
+    code.push_str(&quote_block! {
+        pub trait Packet {
+            fn to_bytes(self) -> Bytes;
+            fn to_vec(self) -> Vec<u8>;
+        }
+    });
+
+    Ok(code)
+}
+
+/// Round up the bit width to a Rust integer size.
+fn round_bit_width(width: usize) -> Result<usize> {
+    match width {
+        8 => Ok(8),
+        16 => Ok(16),
+        24 | 32 => Ok(32),
+        40 | 48 | 56 | 64 => Ok(64),
+        _ => bail!("unsupported field width: {width}"),
+    }
+}
+
+/// Generate a Rust unsigned integer type large enough to hold
+/// integers of the given bit width.
+fn type_for_width(width: usize) -> Result<syn::Type> {
+    let rounded_width = round_bit_width(width)?;
+    syn::parse_str(&format!("u{rounded_width}")).map_err(anyhow::Error::from)
+}
+
+fn generate_field(
+    field: &ast::Field,
+    visibility: syn::Visibility,
+) -> Result<proc_macro2::TokenStream> {
+    match field {
+        ast::Field::Scalar { id, width, .. } => {
+            let field_name = format_ident!("{id}");
+            let field_type = type_for_width(*width)?;
+            Ok(quote! {
+                #visibility #field_name: #field_type
+            })
+        }
+        _ => todo!("unsupported field: {:?}", field),
+    }
+}
+
+fn generate_field_getter(
+    packet_name: &syn::Ident,
+    field: &ast::Field,
+) -> Result<proc_macro2::TokenStream> {
+    match field {
+        ast::Field::Scalar { id, width, .. } => {
+            // TODO(mgeisler): refactor with generate_field above.
+            let getter_name = format_ident!("get_{id}");
+            let field_name = format_ident!("{id}");
+            let field_type = type_for_width(*width)?;
+            Ok(quote! {
+                pub fn #getter_name(&self) -> #field_type {
+                    self.#packet_name.as_ref().#field_name
+                }
+            })
+        }
+        _ => todo!("unsupported field: {:?}", field),
+    }
+}
+
+fn generate_field_parser(
+    endianness_value: &ast::EndiannessValue,
+    packet_name: &str,
+    field: &ast::Field,
+    offset: usize,
+) -> Result<proc_macro2::TokenStream> {
+    match field {
+        ast::Field::Scalar { id, width, .. } => {
+            let field_name = format_ident!("{id}");
+            let type_width = round_bit_width(*width)?;
+            let field_type = type_for_width(*width)?;
+
+            let getter = match endianness_value {
+                ast::EndiannessValue::BigEndian => format_ident!("from_be_bytes"),
+                ast::EndiannessValue::LittleEndian => format_ident!("from_le_bytes"),
+            };
+
+            let wanted_len = syn::Index::from(offset + width / 8);
+            let indices = (offset..offset + width / 8).map(syn::Index::from);
+            let padding = vec![syn::Index::from(0); (type_width - width) / 8];
+            let mask = if *width != type_width {
+                Some(quote! {
+                    let #field_name = #field_name & 0xfff;
+                })
+            } else {
+                None
+            };
+
+            Ok(quote! {
+                // TODO(mgeisler): call a function instead to avoid
+                // generating so much code for this.
+                if bytes.len() < #wanted_len {
+                    return Err(Error::InvalidLengthError {
+                        obj: #packet_name.to_string(),
+                        field: #id.to_string(),
+                        wanted: #wanted_len,
+                        got: bytes.len(),
+                    });
+                }
+                let #field_name = #field_type::#getter([#(bytes[#indices]),* #(, #padding)*]);
+                #mask
+            })
+        }
+        _ => todo!("unsupported field: {:?}", field),
+    }
+}
+
+fn generate_field_writer(
+    grammar: &ast::Grammar,
+    field: &ast::Field,
+    offset: usize,
+) -> Result<proc_macro2::TokenStream> {
+    match field {
+        ast::Field::Scalar { id, width, .. } => {
+            let field_name = format_ident!("{id}");
+            let bit_width = round_bit_width(*width)?;
+            let start = syn::Index::from(offset);
+            let end = syn::Index::from(offset + bit_width / 8);
+            let byte_width = syn::Index::from(bit_width / 8);
+            let writer = match grammar.endianness.value {
+                ast::EndiannessValue::BigEndian => format_ident!("to_be_bytes"),
+                ast::EndiannessValue::LittleEndian => format_ident!("to_le_bytes"),
+            };
+            Ok(quote! {
+                let #field_name = self.#field_name;
+                buffer[#start..#end].copy_from_slice(&#field_name.#writer()[0..#byte_width]);
+            })
+        }
+        _ => todo!("unsupported field: {:?}", field),
+    }
+}
+
+fn get_field_size(field: &ast::Field) -> usize {
+    match field {
+        ast::Field::Scalar { width, .. } => width / 8,
+        _ => todo!("unsupported field: {:?}", field),
+    }
+}
+
+/// Generate code for an `ast::Decl::Packet` enum value.
+fn generate_packet_decl(
+    grammar: &ast::Grammar,
+    packets: &HashMap<&str, &ast::Decl>,
+    child_ids: &[&str],
+    id: &str,
+    fields: &[ast::Field],
+    parent_id: &Option<String>,
+) -> Result<String> {
+    // TODO(mgeisler): use the convert_case crate to convert between
+    // `FooBar` and `foo_bar` in the code below.
+    let mut code = String::new();
+
+    let has_children = !child_ids.is_empty();
+    let child_idents = child_ids.iter().map(|id| format_ident!("{id}")).collect::<Vec<_>>();
+
+    let ident = format_ident!("{}", id.to_lowercase());
+    let data_child_ident = format_ident!("{id}DataChild");
+    let child_decl_packet_name =
+        child_idents.iter().map(|ident| format_ident!("{ident}Packet")).collect::<Vec<_>>();
+    let child_name = format_ident!("{id}Child");
+    if has_children {
+        let child_data_idents = child_idents.iter().map(|ident| format_ident!("{ident}Data"));
+        code.push_str(&quote_block! {
+            #[derive(Debug)]
+            enum #data_child_ident {
+                #(#child_idents(Arc<#child_data_idents>),)*
+                None,
+            }
+
+            impl #data_child_ident {
+                fn get_total_size(&self) -> usize {
+                    // TODO(mgeisler): use Self instad of #data_child_ident.
+                    match self {
+                        #(#data_child_ident::#child_idents(value) => value.get_total_size(),)*
+                        #data_child_ident::None => 0,
+                    }
+                }
+            }
+
+            #[derive(Debug)]
+            pub enum #child_name {
+                #(#child_idents(#child_decl_packet_name),)*
+                None,
+            }
+        });
+    }
+
+    let data_name = format_ident!("{id}Data");
+    let child_field = has_children.then(|| {
+        quote! {
+            child: #data_child_ident,
+        }
+    });
+    let plain_fields = fields
+        .iter()
+        .map(|field| generate_field(field, parse_quote!()))
+        .collect::<Result<Vec<_>>>()?;
+    code.push_str(&quote_block! {
+        #[derive(Debug)]
+        struct #data_name {
+            #(#plain_fields,)*
+            #child_field
+        }
+    });
+
+    let parent = parent_id.as_ref().map(|parent_id| match packets.get(parent_id.as_str()) {
+        Some(ast::Decl::Packet { id, .. }) => {
+            let parent_ident = format_ident!("{}", id.to_lowercase());
+            let parent_data = format_ident!("{id}Data");
+            quote! {
+                #parent_ident: Arc<#parent_data>,
+            }
+        }
+        _ => panic!("Could not find {parent_id}"),
+    });
+
+    let packet_name = format_ident!("{id}Packet");
+    code.push_str(&quote_block! {
+        #[derive(Debug, Clone)]
+        pub struct #packet_name {
+            #parent
+            #ident: Arc<#data_name>,
+        }
+    });
+
+    let builder_name = format_ident!("{id}Builder");
+    let pub_fields = fields
+        .iter()
+        .map(|field| generate_field(field, parse_quote!(pub)))
+        .collect::<Result<Vec<_>>>()?;
+    code.push_str(&quote_block! {
+        #[derive(Debug)]
+        pub struct #builder_name {
+            #(#pub_fields,)*
+        }
+    });
+
+    // TODO(mgeisler): use the `Buf` trait instead of tracking
+    // the offset manually.
+    let mut offset = 0;
+    let field_parsers = fields
+        .iter()
+        .map(|field| {
+            let parser = generate_field_parser(&grammar.endianness.value, id, field, offset);
+            offset += get_field_size(field);
+            parser
+        })
+        .collect::<Result<Vec<_>>>()?;
+    let field_names = fields
+        .iter()
+        .map(|field| match field {
+            ast::Field::Scalar { id, .. } => format_ident!("{id}"),
+            _ => todo!("unsupported field: {:?}", field),
+        })
+        .collect::<Vec<_>>();
+    let mut offset = 0;
+    let field_writers = fields
+        .iter()
+        .map(|field| {
+            let writer = generate_field_writer(grammar, field, offset);
+            offset += get_field_size(field);
+            writer
+        })
+        .collect::<Result<Vec<_>>>()?;
+    let total_field_size = syn::Index::from(fields.iter().map(get_field_size).sum::<usize>());
+
+    code.push_str(&quote_block! {
+        impl #data_name {
+            fn conforms(bytes: &[u8]) -> bool {
+                true
+            }
+
+            fn parse(bytes: &[u8]) -> Result<Self> {
+                #(#field_parsers)*
+                Ok(Self { #(#field_names),* })
+            }
+
+            fn write_to(&self, buffer: &mut BytesMut) {
+                #(#field_writers)*
+            }
+
+            fn get_total_size(&self) -> usize {
+                self.get_size()
+            }
+
+            fn get_size(&self) -> usize {
+                let ret = 0;
+                let ret = ret + #total_field_size;
+                ret
+            }
+        }
+    });
+
+    code.push_str(&quote_block! {
+        impl Packet for #packet_name {
+            fn to_bytes(self) -> Bytes {
+                let mut buffer = BytesMut::new();
+                buffer.resize(self.#ident.get_total_size(), 0);
+                self.#ident.write_to(&mut buffer);
+                buffer.freeze()
+            }
+            fn to_vec(self) -> Vec<u8> {
+                self.to_bytes().to_vec()
+            }
+        }
+        impl From<#packet_name> for Bytes {
+            fn from(packet: #packet_name) -> Self {
+                packet.to_bytes()
+            }
+        }
+        impl From<#packet_name> for Vec<u8> {
+            fn from(packet: #packet_name) -> Self {
+                packet.to_vec()
+            }
+        }
+    });
+
+    let specialize = has_children.then(|| {
+        quote! {
+            pub fn specialize(&self) -> #child_name {
+                match &self.#ident.child {
+                    #(#data_child_ident::#child_idents(_) =>
+                      #child_name::#child_idents(
+                          #child_decl_packet_name::new(self.#ident.clone()).unwrap()),)*
+                    #data_child_ident::None => #child_name::None,
+                }
+            }
+        }
+    });
+    let field_getters = fields
+        .iter()
+        .map(|field| generate_field_getter(&ident, field))
+        .collect::<Result<Vec<_>>>()?;
+    code.push_str(&quote_block! {
+        impl #packet_name {
+            pub fn parse(bytes: &[u8]) -> Result<Self> {
+                Ok(Self::new(Arc::new(#data_name::parse(bytes)?)).unwrap())
+            }
+
+            #specialize
+
+            fn new(root: Arc<#data_name>) -> std::result::Result<Self, &'static str> {
+                let #ident = root;
+                Ok(Self { #ident })
+            }
+
+            #(#field_getters)*
+        }
+    });
+
+    let child = has_children.then(|| {
+        quote! {
+            child: #data_child_ident::None,
+        }
+    });
+    code.push_str(&quote_block! {
+        impl #builder_name {
+            pub fn build(self) -> #packet_name {
+                let #ident = Arc::new(#data_name {
+                    #(#field_names: self.#field_names,)*
+                    #child
+                });
+                #packet_name::new(#ident).unwrap()
+            }
+        }
+    });
+
+    Ok(code)
+}
+
+fn generate_decl(
+    grammar: &ast::Grammar,
+    packets: &HashMap<&str, &ast::Decl>,
+    children: &HashMap<&str, Vec<&str>>,
+    decl: &ast::Decl,
+) -> Result<String> {
+    let empty: Vec<&str> = vec![];
+    match decl {
+        ast::Decl::Packet { id, fields, parent_id, .. } => generate_packet_decl(
+            grammar,
+            packets,
+            children.get(id.as_str()).unwrap_or(&empty),
+            id,
+            fields,
+            parent_id,
+        ),
+        _ => todo!("unsupported Decl::{:?}", decl),
+    }
+}
+
+/// Generate Rust code from `grammar`.
+///
+/// The code is not formatted, pipe it through `rustfmt` to get
+/// readable source code.
+pub fn generate_rust(sources: &ast::SourceDatabase, grammar: &ast::Grammar) -> Result<String> {
+    let source =
+        sources.get(grammar.file).with_context(|| format!("could not read {}", grammar.file))?;
+
+    let mut children = HashMap::new();
+    let mut packets = HashMap::new();
+    for decl in &grammar.declarations {
+        if let ast::Decl::Packet { id, parent_id, .. } = decl {
+            packets.insert(id.as_str(), decl);
+            if let Some(parent_id) = parent_id {
+                children.entry(parent_id.as_str()).or_insert_with(Vec::new).push(id.as_str());
+            }
+        }
+    }
+
+    let mut code = String::new();
+
+    code.push_str(&generate_preamble(Path::new(source.name()))?);
+
+    for decl in &grammar.declarations {
+        let decl_code = generate_decl(grammar, &packets, &children, decl)
+            .with_context(|| format!("failed to generating code for {:?}", decl))?;
+        code.push_str(&decl_code);
+        code.push_str("\n\n");
+    }
+
+    Ok(code)
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::parser::parse_inline;
+    use std::io::Write;
+    use std::process::Command;
+    use std::process::Stdio;
+    use tempfile::NamedTempFile;
+
+    fn parse(text: &str) -> ast::Grammar {
+        let mut db = ast::SourceDatabase::new();
+        parse_inline(&mut db, String::from("stdin"), String::from(text)).expect("parsing failure")
+    }
+
+    fn format_with_rustfmt(unformatted: &str) -> String {
+        // We expect to find `rustfmt` as a sibling to the test
+        // executable. It ends up there when referenced using the
+        // `data` property in an Android.pb file.
+        let mut rustfmt_path = std::env::current_exe().unwrap();
+        rustfmt_path.set_file_name("rustfmt");
+        let mut rustfmt = Command::new(&rustfmt_path)
+            .stdin(Stdio::piped())
+            .stdout(Stdio::piped())
+            .spawn()
+            .unwrap_or_else(|_| panic!("failed to start {:?}", &rustfmt_path));
+
+        let mut stdin = rustfmt.stdin.take().unwrap();
+        // Owned copy which we can move into the writing thread.
+        let unformatted = String::from(unformatted);
+        std::thread::spawn(move || {
+            stdin.write_all(unformatted.as_bytes()).expect("could not write to stdin");
+        });
+
+        let output = rustfmt.wait_with_output().expect("error executing rustfmt");
+        assert!(output.status.success(), "rustfmt failed: {}", output.status);
+        String::from_utf8(output.stdout).expect("rustfmt output was not UTF-8")
+    }
+
+    fn unified_diff(left: &str, right: &str) -> String {
+        let mut temp_left = NamedTempFile::new().unwrap();
+        temp_left.write_all(left.as_bytes()).unwrap();
+        let mut temp_right = NamedTempFile::new().unwrap();
+        temp_right.write_all(right.as_bytes()).unwrap();
+
+        // We expect `diff` to be available on PATH.
+        let output = Command::new("diff")
+            .arg("--unified")
+            .arg("--label")
+            .arg("left")
+            .arg("--label")
+            .arg("right")
+            .arg(temp_left.path())
+            .arg(temp_right.path())
+            .output()
+            .expect("failed to run diff");
+        let diff_trouble_exit_code = 2; // from diff(1)
+        assert_ne!(
+            output.status.code().unwrap(),
+            diff_trouble_exit_code,
+            "diff failed: {}",
+            output.status
+        );
+        String::from_utf8(output.stdout).expect("diff output was not UTF-8")
+    }
+
+    #[track_caller]
+    fn assert_eq_with_diff(left: &str, right: &str) {
+        assert!(
+            left == right,
+            "texts did not match, left:\n{}\n\n\
+             right:\n{}\n\n\
+             diff:\n{}\n",
+            left,
+            right,
+            unified_diff(left, right)
+        );
+    }
+
+    #[test]
+    fn test_generate_preamble() {
+        let actual_code = generate_preamble(Path::new("some/path/foo.pdl")).unwrap();
+        let expected_code = include_str!("../test/generated/preamble.rs");
+        assert_eq_with_diff(&format_with_rustfmt(&actual_code), expected_code);
+    }
+
+    #[test]
+    fn test_generate_packet_decl_empty() {
+        let grammar = parse(
+            r#"
+              big_endian_packets
+              packet Foo {}
+            "#,
+        );
+        let packets = HashMap::new();
+        let children = HashMap::new();
+        let decl = &grammar.declarations[0];
+        let actual_code = generate_decl(&grammar, &packets, &children, decl).unwrap();
+        let expected_code = include_str!("../test/generated/packet_decl_empty.rs");
+        assert_eq_with_diff(&format_with_rustfmt(&actual_code), expected_code);
+    }
+
+    #[test]
+    fn test_generate_packet_decl_little_endian() {
+        let grammar = parse(
+            r#"
+              little_endian_packets
+
+              packet Foo {
+                x: 8,
+                y: 16,
+              }
+            "#,
+        );
+        let packets = HashMap::new();
+        let children = HashMap::new();
+        let decl = &grammar.declarations[0];
+        let actual_code = generate_decl(&grammar, &packets, &children, decl).unwrap();
+        let expected_code = include_str!("../test/generated/packet_decl_simple_little_endian.rs");
+        assert_eq_with_diff(&format_with_rustfmt(&actual_code), expected_code);
+    }
+
+    #[test]
+    fn test_generate_packet_decl_simple_big_endian() {
+        let grammar = parse(
+            r#"
+              big_endian_packets
+
+              packet Foo {
+                x: 8,
+                y: 16,
+              }
+            "#,
+        );
+        let packets = HashMap::new();
+        let children = HashMap::new();
+        let decl = &grammar.declarations[0];
+        let actual_code = generate_decl(&grammar, &packets, &children, decl).unwrap();
+        let expected_code = include_str!("../test/generated/packet_decl_simple_big_endian.rs");
+        assert_eq_with_diff(&format_with_rustfmt(&actual_code), expected_code);
+    }
+}
diff --git a/tools/pdl/src/main.rs b/tools/pdl/src/main.rs
index ff5c585..7bd8c03 100644
--- a/tools/pdl/src/main.rs
+++ b/tools/pdl/src/main.rs
@@ -4,11 +4,30 @@
 use structopt::StructOpt;
 
 mod ast;
+mod generator;
 mod lint;
 mod parser;
 
 use crate::lint::Lintable;
 
+#[derive(Debug)]
+enum OutputFormat {
+    JSON,
+    Rust,
+}
+
+impl std::str::FromStr for OutputFormat {
+    type Err = String;
+
+    fn from_str(input: &str) -> Result<Self, Self::Err> {
+        match input.to_lowercase().as_str() {
+            "json" => Ok(Self::JSON),
+            "rust" => Ok(Self::Rust),
+            _ => Err(format!("could not parse {:?}, valid option are 'json' and 'rust'.", input)),
+        }
+    }
+}
+
 #[derive(Debug, StructOpt)]
 #[structopt(name = "pdl-parser", about = "Packet Description Language parser tool.")]
 struct Opt {
@@ -16,6 +35,11 @@
     #[structopt(short, long = "--version")]
     version: bool,
 
+    /// Generate output in this format ("json" or "rust"). The output
+    /// will be printed on stdout in both cases.
+    #[structopt(short, long = "--output-format", name = "FORMAT", default_value = "JSON")]
+    output_format: OutputFormat,
+
     /// Input file.
     #[structopt(name = "FILE")]
     input_file: String,
@@ -33,7 +57,16 @@
     match parser::parse_file(&mut sources, opt.input_file) {
         Ok(grammar) => {
             let _ = grammar.lint().print(&sources, termcolor::ColorChoice::Always);
-            println!("{}", serde_json::to_string_pretty(&grammar).unwrap())
+
+            match opt.output_format {
+                OutputFormat::JSON => {
+                    println!("{}", serde_json::to_string_pretty(&grammar).unwrap())
+                }
+                OutputFormat::Rust => match generator::generate_rust(&sources, &grammar) {
+                    Ok(code) => println!("{}", &code),
+                    Err(err) => println!("failed to generate code: {}", err),
+                },
+            }
         }
         Err(err) => {
             let writer = termcolor::StandardStream::stderr(termcolor::ColorChoice::Always);
diff --git a/tools/pdl/src/parser.rs b/tools/pdl/src/parser.rs
index edeecfe..dd4e702 100644
--- a/tools/pdl/src/parser.rs
+++ b/tools/pdl/src/parser.rs
@@ -187,7 +187,7 @@
     Err(format!("expected rule {:?}, got nothing", expected))
 }
 
-fn expect<'i>(iter: &mut NodeIterator<'i>, rule: Rule) -> Result<Node<'i>, String> {
+fn expect<'i>(iter: &mut impl Iterator<Item = Node<'i>>, rule: Rule) -> Result<Node<'i>, String> {
     match iter.next() {
         Some(node) if node.as_rule() == rule => Ok(node),
         Some(node) => err_unexpected_rule(rule, node.as_rule()),
@@ -233,8 +233,12 @@
     }
 }
 
-fn parse_string(iter: &mut NodeIterator<'_>) -> Result<String, String> {
-    expect(iter, Rule::string).map(|n| n.as_string())
+fn parse_string<'i>(iter: &mut impl Iterator<Item = Node<'i>>) -> Result<String, String> {
+    expect(iter, Rule::string)
+        .map(|n| n.as_str())
+        .and_then(|s| s.strip_prefix('"').ok_or_else(|| "expected \" prefix".to_owned()))
+        .and_then(|s| s.strip_suffix('"').ok_or_else(|| "expected \" suffix".to_owned()))
+        .map(|s| s.to_owned())
 }
 
 fn parse_size_modifier_opt(iter: &mut NodeIterator<'_>) -> Option<String> {
@@ -533,4 +537,29 @@
         assert_eq!(grammar.endianness.value, ast::EndiannessValue::BigEndian);
         assert_ne!(grammar.endianness.loc, ast::SourceRange::default());
     }
+
+    #[test]
+    fn test_parse_string_bare() {
+        let mut pairs = PDLParser::parse(Rule::string, r#""test""#).unwrap();
+
+        assert_eq!(parse_string(&mut pairs).as_deref(), Ok("test"));
+        assert_eq!(pairs.next(), None, "pairs is empty");
+    }
+
+    #[test]
+    fn test_parse_string_space() {
+        let mut pairs = PDLParser::parse(Rule::string, r#""test with space""#).unwrap();
+
+        assert_eq!(parse_string(&mut pairs).as_deref(), Ok("test with space"));
+        assert_eq!(pairs.next(), None, "pairs is empty");
+    }
+
+    #[test]
+    #[should_panic] /* This is not supported */
+    fn test_parse_string_escape() {
+        let mut pairs = PDLParser::parse(Rule::string, r#""\"test\"""#).unwrap();
+
+        assert_eq!(parse_string(&mut pairs).as_deref(), Ok(r#""test""#));
+        assert_eq!(pairs.next(), None, "pairs is empty");
+    }
 }
diff --git a/tools/pdl/test/generated/packet_decl_empty.rs b/tools/pdl/test/generated/packet_decl_empty.rs
new file mode 100644
index 0000000..f4a8dcf
--- /dev/null
+++ b/tools/pdl/test/generated/packet_decl_empty.rs
@@ -0,0 +1,67 @@
+#[derive(Debug)]
+struct FooData {}
+
+#[derive(Debug, Clone)]
+pub struct FooPacket {
+    foo: Arc<FooData>,
+}
+
+#[derive(Debug)]
+pub struct FooBuilder {}
+
+impl FooData {
+    fn conforms(bytes: &[u8]) -> bool {
+        true
+    }
+    fn parse(bytes: &[u8]) -> Result<Self> {
+        Ok(Self {})
+    }
+    fn write_to(&self, buffer: &mut BytesMut) {}
+    fn get_total_size(&self) -> usize {
+        self.get_size()
+    }
+    fn get_size(&self) -> usize {
+        let ret = 0;
+        let ret = ret + 0;
+        ret
+    }
+}
+
+impl Packet for FooPacket {
+    fn to_bytes(self) -> Bytes {
+        let mut buffer = BytesMut::new();
+        buffer.resize(self.foo.get_total_size(), 0);
+        self.foo.write_to(&mut buffer);
+        buffer.freeze()
+    }
+    fn to_vec(self) -> Vec<u8> {
+        self.to_bytes().to_vec()
+    }
+}
+impl From<FooPacket> for Bytes {
+    fn from(packet: FooPacket) -> Self {
+        packet.to_bytes()
+    }
+}
+impl From<FooPacket> for Vec<u8> {
+    fn from(packet: FooPacket) -> Self {
+        packet.to_vec()
+    }
+}
+
+impl FooPacket {
+    pub fn parse(bytes: &[u8]) -> Result<Self> {
+        Ok(Self::new(Arc::new(FooData::parse(bytes)?)).unwrap())
+    }
+    fn new(root: Arc<FooData>) -> std::result::Result<Self, &'static str> {
+        let foo = root;
+        Ok(Self { foo })
+    }
+}
+
+impl FooBuilder {
+    pub fn build(self) -> FooPacket {
+        let foo = Arc::new(FooData {});
+        FooPacket::new(foo).unwrap()
+    }
+}
diff --git a/tools/pdl/test/generated/packet_decl_simple_big_endian.rs b/tools/pdl/test/generated/packet_decl_simple_big_endian.rs
new file mode 100644
index 0000000..89b1bab
--- /dev/null
+++ b/tools/pdl/test/generated/packet_decl_simple_big_endian.rs
@@ -0,0 +1,102 @@
+#[derive(Debug)]
+struct FooData {
+    x: u8,
+    y: u16,
+}
+
+#[derive(Debug, Clone)]
+pub struct FooPacket {
+    foo: Arc<FooData>,
+}
+
+#[derive(Debug)]
+pub struct FooBuilder {
+    pub x: u8,
+    pub y: u16,
+}
+
+impl FooData {
+    fn conforms(bytes: &[u8]) -> bool {
+        true
+    }
+    fn parse(bytes: &[u8]) -> Result<Self> {
+        if bytes.len() < 1 {
+            return Err(Error::InvalidLengthError {
+                obj: "Foo".to_string(),
+                field: "x".to_string(),
+                wanted: 1,
+                got: bytes.len(),
+            });
+        }
+        let x = u8::from_be_bytes([bytes[0]]);
+        if bytes.len() < 3 {
+            return Err(Error::InvalidLengthError {
+                obj: "Foo".to_string(),
+                field: "y".to_string(),
+                wanted: 3,
+                got: bytes.len(),
+            });
+        }
+        let y = u16::from_be_bytes([bytes[1], bytes[2]]);
+        Ok(Self { x, y })
+    }
+    fn write_to(&self, buffer: &mut BytesMut) {
+        let x = self.x;
+        buffer[0..1].copy_from_slice(&x.to_be_bytes()[0..1]);
+        let y = self.y;
+        buffer[1..3].copy_from_slice(&y.to_be_bytes()[0..2]);
+    }
+    fn get_total_size(&self) -> usize {
+        self.get_size()
+    }
+    fn get_size(&self) -> usize {
+        let ret = 0;
+        let ret = ret + 3;
+        ret
+    }
+}
+
+impl Packet for FooPacket {
+    fn to_bytes(self) -> Bytes {
+        let mut buffer = BytesMut::new();
+        buffer.resize(self.foo.get_total_size(), 0);
+        self.foo.write_to(&mut buffer);
+        buffer.freeze()
+    }
+    fn to_vec(self) -> Vec<u8> {
+        self.to_bytes().to_vec()
+    }
+}
+impl From<FooPacket> for Bytes {
+    fn from(packet: FooPacket) -> Self {
+        packet.to_bytes()
+    }
+}
+impl From<FooPacket> for Vec<u8> {
+    fn from(packet: FooPacket) -> Self {
+        packet.to_vec()
+    }
+}
+
+impl FooPacket {
+    pub fn parse(bytes: &[u8]) -> Result<Self> {
+        Ok(Self::new(Arc::new(FooData::parse(bytes)?)).unwrap())
+    }
+    fn new(root: Arc<FooData>) -> std::result::Result<Self, &'static str> {
+        let foo = root;
+        Ok(Self { foo })
+    }
+    pub fn get_x(&self) -> u8 {
+        self.foo.as_ref().x
+    }
+    pub fn get_y(&self) -> u16 {
+        self.foo.as_ref().y
+    }
+}
+
+impl FooBuilder {
+    pub fn build(self) -> FooPacket {
+        let foo = Arc::new(FooData { x: self.x, y: self.y });
+        FooPacket::new(foo).unwrap()
+    }
+}
diff --git a/tools/pdl/test/generated/packet_decl_simple_little_endian.rs b/tools/pdl/test/generated/packet_decl_simple_little_endian.rs
new file mode 100644
index 0000000..5b81ec8
--- /dev/null
+++ b/tools/pdl/test/generated/packet_decl_simple_little_endian.rs
@@ -0,0 +1,102 @@
+#[derive(Debug)]
+struct FooData {
+    x: u8,
+    y: u16,
+}
+
+#[derive(Debug, Clone)]
+pub struct FooPacket {
+    foo: Arc<FooData>,
+}
+
+#[derive(Debug)]
+pub struct FooBuilder {
+    pub x: u8,
+    pub y: u16,
+}
+
+impl FooData {
+    fn conforms(bytes: &[u8]) -> bool {
+        true
+    }
+    fn parse(bytes: &[u8]) -> Result<Self> {
+        if bytes.len() < 1 {
+            return Err(Error::InvalidLengthError {
+                obj: "Foo".to_string(),
+                field: "x".to_string(),
+                wanted: 1,
+                got: bytes.len(),
+            });
+        }
+        let x = u8::from_le_bytes([bytes[0]]);
+        if bytes.len() < 3 {
+            return Err(Error::InvalidLengthError {
+                obj: "Foo".to_string(),
+                field: "y".to_string(),
+                wanted: 3,
+                got: bytes.len(),
+            });
+        }
+        let y = u16::from_le_bytes([bytes[1], bytes[2]]);
+        Ok(Self { x, y })
+    }
+    fn write_to(&self, buffer: &mut BytesMut) {
+        let x = self.x;
+        buffer[0..1].copy_from_slice(&x.to_le_bytes()[0..1]);
+        let y = self.y;
+        buffer[1..3].copy_from_slice(&y.to_le_bytes()[0..2]);
+    }
+    fn get_total_size(&self) -> usize {
+        self.get_size()
+    }
+    fn get_size(&self) -> usize {
+        let ret = 0;
+        let ret = ret + 3;
+        ret
+    }
+}
+
+impl Packet for FooPacket {
+    fn to_bytes(self) -> Bytes {
+        let mut buffer = BytesMut::new();
+        buffer.resize(self.foo.get_total_size(), 0);
+        self.foo.write_to(&mut buffer);
+        buffer.freeze()
+    }
+    fn to_vec(self) -> Vec<u8> {
+        self.to_bytes().to_vec()
+    }
+}
+impl From<FooPacket> for Bytes {
+    fn from(packet: FooPacket) -> Self {
+        packet.to_bytes()
+    }
+}
+impl From<FooPacket> for Vec<u8> {
+    fn from(packet: FooPacket) -> Self {
+        packet.to_vec()
+    }
+}
+
+impl FooPacket {
+    pub fn parse(bytes: &[u8]) -> Result<Self> {
+        Ok(Self::new(Arc::new(FooData::parse(bytes)?)).unwrap())
+    }
+    fn new(root: Arc<FooData>) -> std::result::Result<Self, &'static str> {
+        let foo = root;
+        Ok(Self { foo })
+    }
+    pub fn get_x(&self) -> u8 {
+        self.foo.as_ref().x
+    }
+    pub fn get_y(&self) -> u16 {
+        self.foo.as_ref().y
+    }
+}
+
+impl FooBuilder {
+    pub fn build(self) -> FooPacket {
+        let foo = Arc::new(FooData { x: self.x, y: self.y });
+        FooPacket::new(foo).unwrap()
+    }
+}
diff --git a/tools/pdl/test/generated/preamble.rs b/tools/pdl/test/generated/preamble.rs
new file mode 100644
index 0000000..0a03fe4
--- /dev/null
+++ b/tools/pdl/test/generated/preamble.rs
@@ -0,0 +1,34 @@
+// @generated rust packets from foo.pdl
+
+use bytes::{BufMut, Bytes, BytesMut};
+use num_derive::{FromPrimitive, ToPrimitive};
+use num_traits::{FromPrimitive, ToPrimitive};
+use std::convert::{TryFrom, TryInto};
+use std::fmt;
+use std::sync::Arc;
+use thiserror::Error;
+
+type Result<T> = std::result::Result<T, Error>;
+
+#[derive(Debug, Error)]
+pub enum Error {
+    #[error("Packet parsing failed")]
+    InvalidPacketError,
+    #[error("{field} was {value:x}, which is not known")]
+    ConstraintOutOfBounds { field: String, value: u64 },
+    #[error("when parsing {obj}.{field} needed length of {wanted} but got {got}")]
+    InvalidLengthError { obj: String, field: String, wanted: usize, got: usize },
+    #[error("Due to size restrictions a struct could not be parsed.")]
+    ImpossibleStructError,
+    #[error("when parsing field {obj}.{field}, {value} is not a valid {type_} value")]
+    InvalidEnumValueError { obj: String, field: String, value: u64, type_: String },
+}
+
+#[derive(Debug, Error)]
+#[error("{0}")]
+pub struct TryFromError(&'static str);
+
+pub trait Packet {
+    fn to_bytes(self) -> Bytes;
+    fn to_vec(self) -> Vec<u8>;
+}
diff --git a/tools/rootcanal/Android.bp b/tools/rootcanal/Android.bp
index 2e705bf..2c62d8d 100644
--- a/tools/rootcanal/Android.bp
+++ b/tools/rootcanal/Android.bp
@@ -124,6 +124,8 @@
         "clang_file_coverage",
         "clang_coverage_bin",
     ],
+    // TODO(b/231993739): Reenable isolated:true by deleting the explicit disable below
+    isolated: false,
     srcs: [
         "test/async_manager_unittest.cc",
         "test/h4_parser_unittest.cc",
diff --git a/tools/rootcanal/model/controller/link_layer_controller.cc b/tools/rootcanal/model/controller/link_layer_controller.cc
index e706681..6155c86 100644
--- a/tools/rootcanal/model/controller/link_layer_controller.cc
+++ b/tools/rootcanal/model/controller/link_layer_controller.cc
@@ -764,14 +764,19 @@
              peer.ToString().c_str());
     return;
   }
+#ifdef ROOTCANAL_LMP
+  auto is_br_edr = connections_.GetPhyType(handle) == Phy::Type::BR_EDR;
+#endif
   ASSERT_LOG(connections_.Disconnect(handle),
              "GetHandle() returned invalid handle %hx", handle);
 
   uint8_t reason = disconnect.GetReason();
   SendDisconnectionCompleteEvent(handle, reason);
 #ifdef ROOTCANAL_LMP
-  ASSERT(link_manager_remove_link(
-      lm_.get(), reinterpret_cast<uint8_t(*)[6]>(peer.data())));
+  if (is_br_edr) {
+    ASSERT(link_manager_remove_link(
+        lm_.get(), reinterpret_cast<uint8_t(*)[6]>(peer.data())));
+  }
 #endif
 }
 
@@ -2902,8 +2907,9 @@
   }
 
   const AddressWithType remote = connections_.GetAddress(handle);
+  auto is_br_edr = connections_.GetPhyType(handle) == Phy::Type::BR_EDR;
 
-  if (connections_.GetPhyType(handle) == Phy::Type::BR_EDR) {
+  if (is_br_edr) {
     LOG_INFO("Disconnecting ACL connection with %s", remote.ToString().c_str());
 
     uint16_t sco_handle = connections_.GetScoHandle(remote.GetAddress());
@@ -2929,8 +2935,11 @@
   connections_.Disconnect(handle);
   SendDisconnectionCompleteEvent(handle, reason);
 #ifdef ROOTCANAL_LMP
-  ASSERT(link_manager_remove_link(
-      lm_.get(), reinterpret_cast<uint8_t(*)[6]>(remote.GetAddress().data())));
+  if (is_br_edr) {
+    ASSERT(link_manager_remove_link(
+        lm_.get(),
+        reinterpret_cast<uint8_t(*)[6]>(remote.GetAddress().data())));
+  }
 #endif
   return ErrorCode::SUCCESS;
 }
diff --git a/tools/rootcanal/model/setup/async_manager.cc b/tools/rootcanal/model/setup/async_manager.cc
index 01447ec..5ae004a 100644
--- a/tools/rootcanal/model/setup/async_manager.cc
+++ b/tools/rootcanal/model/setup/async_manager.cc
@@ -364,6 +364,7 @@
     std::chrono::steady_clock::time_point time;
     bool periodic;
     std::chrono::milliseconds period{};
+    std::mutex in_callback; // Taken when the callback is active
     TaskCallback callback;
     AsyncTaskId task_id;
     AsyncUserId user_id;
@@ -381,8 +382,22 @@
     if (tasks_by_id_.count(async_task_id) == 0) {
       return false;
     }
-    task_queue_.erase(tasks_by_id_[async_task_id]);
-    tasks_by_id_.erase(async_task_id);
+
+    // Now make sure we are not running this task.
+    // 2 cases:
+    // - This is called from thread_, this means a running
+    //   scheduled task is actually unregistering. All bets are off.
+    // - Another thread is calling us, let's make sure the task is not active.
+    if (thread_.get_id() != std::this_thread::get_id()) {
+      auto task = tasks_by_id_[async_task_id];
+      const std::lock_guard<std::mutex> lock(task->in_callback);
+      task_queue_.erase(task);
+      tasks_by_id_.erase(async_task_id);
+    } else {
+      task_queue_.erase(tasks_by_id_[async_task_id]);
+      tasks_by_id_.erase(async_task_id);
+    }
+
     return true;
   }
 
@@ -437,11 +452,12 @@
   void ThreadRoutine() {
     while (running_) {
       TaskCallback callback;
+      std::shared_ptr<Task> task_p;
       bool run_it = false;
       {
         std::unique_lock<std::mutex> guard(internal_mutex_);
         if (!task_queue_.empty()) {
-          std::shared_ptr<Task> task_p = *(task_queue_.begin());
+          task_p = *(task_queue_.begin());
           if (task_p->time < std::chrono::steady_clock::now()) {
             run_it = true;
             callback = task_p->callback;
@@ -458,6 +474,7 @@
         }
       }
       if (run_it) {
+        const std::lock_guard<std::mutex> lock(task_p->in_callback);
         callback();
       }
       {
diff --git a/tools/rootcanal/model/setup/async_manager.h b/tools/rootcanal/model/setup/async_manager.h
index f6603c4..e82fa37 100644
--- a/tools/rootcanal/model/setup/async_manager.h
+++ b/tools/rootcanal/model/setup/async_manager.h
@@ -77,18 +77,18 @@
                                     std::chrono::milliseconds period,
                                     const TaskCallback& callback);
 
-  // Cancels the/every future occurrence of the action specified by this id. It
-  // is guaranteed that the associated callback will not be called after this
-  // method returns (it could be called during the execution of the method).
-  // The calling thread may block until the scheduling thread acknowledges the
-  // cancellation.
+  // Cancels the/every future occurrence of the action specified by this id.
+  // The following invariants will hold:
+  // - The task will not be invoked after this method returns
+  // - If the task is currently running it will block until the task is
+  //   completed, unless cancel is called from the running task.
   bool CancelAsyncTask(AsyncTaskId async_task_id);
 
-  // Cancels the/every future occurrence of the action specified by this id. It
-  // is guaranteed that the associated callback will not be called after this
-  // method returns (it could be called during the execution of the method).
-  // The calling thread may block until the scheduling thread acknowledges the
-  // cancellation.
+  // Cancels the/every future occurrence of the action specified by this id.
+  // The following invariants will hold:
+  // - The task will not be invoked after this method returns
+  // - If the task is currently running it will block until the task is
+  //   completed, unless cancel is called from the running task.
   bool CancelAsyncTasksFromUser(AsyncUserId user_id);
 
   // Execs the given code in a synchronized manner. It is guaranteed that code
diff --git a/tools/rootcanal/model/setup/test_model.cc b/tools/rootcanal/model/setup/test_model.cc
index 2746180..470734c 100644
--- a/tools/rootcanal/model/setup/test_model.cc
+++ b/tools/rootcanal/model/setup/test_model.cc
@@ -52,6 +52,10 @@
   model_user_id_ = get_user_id_();
 }
 
+TestModel::~TestModel() {
+  StopTimer();
+}
+
 void TestModel::SetTimerPeriod(std::chrono::milliseconds new_period) {
   timer_period_ = new_period;
 
diff --git a/tools/rootcanal/model/setup/test_model.h b/tools/rootcanal/model/setup/test_model.h
index 263346c..2171c53 100644
--- a/tools/rootcanal/model/setup/test_model.h
+++ b/tools/rootcanal/model/setup/test_model.h
@@ -49,7 +49,7 @@
       std::function<void(AsyncTaskId)> cancel,
       std::function<std::shared_ptr<Device>(const std::string&, int, Phy::Type)>
           connect_to_remote);
-  ~TestModel() = default;
+  ~TestModel();
 
   TestModel(TestModel& model) = delete;
   TestModel& operator=(const TestModel& model) = delete;
diff --git a/tools/rootcanal/test/async_manager_unittest.cc b/tools/rootcanal/test/async_manager_unittest.cc
index a0a33d4..c7a7444 100644
--- a/tools/rootcanal/test/async_manager_unittest.cc
+++ b/tools/rootcanal/test/async_manager_unittest.cc
@@ -33,6 +33,7 @@
 #include <ratio>               // for ratio
 #include <string>              // for string
 #include <tuple>               // for tuple
+#include <thread>
 
 #include "osi/include/osi.h"  // for OSI_NO_INTR
 
@@ -123,6 +124,7 @@
 
   void SetUp() override {
     memset(server_buffer_, 0, kBufferSize);
+    memset(client_buffer_, 0, kBufferSize);
 
     socket_fd_ = StartServer();
 
@@ -137,7 +139,7 @@
   void TearDown() override {
     async_manager_.StopWatchingFileDescriptor(socket_fd_);
     close(socket_fd_);
-    ASSERT_TRUE(CheckBufferEquals());
+    ASSERT_EQ(std::string_view(server_buffer_, kBufferSize), std::string_view(client_buffer_, kBufferSize));
   }
 
   int ConnectClient() {
@@ -215,6 +217,46 @@
   close(socket_cli_fd);
 }
 
+
+TEST_F(AsyncManagerSocketTest, CanUnsubscribeTaskFromWithinTask) {
+  Event running;
+  using namespace std::chrono_literals;
+  async_manager_.ExecAsyncPeriodically(1, 1ms, 2ms, [&running, this]() {
+     EXPECT_TRUE(async_manager_.CancelAsyncTask(1)) << "We were scheduled, so cancel should return true";
+     EXPECT_FALSE(async_manager_.CancelAsyncTask(1)) << "We were not scheduled, so cancel should return false";
+     running.set(true);
+  });
+
+  EXPECT_TRUE(running.wait_for(10ms));
+}
+
+
+TEST_F(AsyncManagerSocketTest, UnsubScribeWaitsUntilCompletion) {
+  using namespace std::chrono_literals;
+  Event running;
+  bool cancel_done = false;
+  bool task_complete = false;
+  async_manager_.ExecAsyncPeriodically(1, 1ms, 2ms, [&running, &cancel_done, &task_complete]() {
+      // Let the other thread now we are in the callback..
+      running.set(true);
+      // Wee bit of a hack that relies on timing..
+      std::this_thread::sleep_for(20ms);
+      EXPECT_FALSE(cancel_done) << "Task cancellation did not wait for us to complete!";
+      task_complete = true;
+  });
+
+  EXPECT_TRUE(running.wait_for(10ms));
+  auto start = std::chrono::system_clock::now();
+
+  // There is a 20ms wait.. so we know that this should take some time.
+  EXPECT_TRUE(async_manager_.CancelAsyncTask(1)) << "We were scheduled, so cancel should return true";
+  cancel_done = true;
+  EXPECT_TRUE(task_complete) << "We managed to cancel a task while it was not yet finished.";
+  auto end = std::chrono::system_clock::now();
+  auto passed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
+  EXPECT_GT(passed_ms.count(), 10);
+}
+
 TEST_F(AsyncManagerSocketTest, NoEventsAfterUnsubscribe) {
   // This tests makes sure the AsyncManager never fires an event
   // after calling StopWatchingFileDescriptor.