Merge "Gd shim updates"
diff --git a/gd/shim/il2cap.h b/gd/shim/il2cap.h
index c4e7463..994953e 100644
--- a/gd/shim/il2cap.h
+++ b/gd/shim/il2cap.h
@@ -28,25 +28,25 @@
 namespace shim {
 
 using ConnectionClosedCallback = std::function<void(uint16_t cid, int error_code)>;
-using Postable = std::function<void(std::function<void(uint16_t cid)>)>;
-using ConnectionOpenCallback = std::function<void(uint16_t psm, uint16_t cid, Postable postable)>;
+using ConnectionOpenCallback = std::function<void(std::string string_address, uint16_t psm, uint16_t cid)>;
 using ReadDataReadyCallback = std::function<void(uint16_t cid, std::vector<const uint8_t> data)>;
 
 struct IL2cap {
   virtual void RegisterService(uint16_t psm, ConnectionOpenCallback on_open, std::promise<void> completed) = 0;
   virtual void UnregisterService(uint16_t psm) = 0;
 
-  virtual void CreateConnection(uint16_t psm, const std::string address, std::promise<uint16_t> completed) = 0;
+  virtual void CreateConnection(uint16_t psm, const std::string address, ConnectionOpenCallback on_open,
+                                std::promise<uint16_t> completed) = 0;
   virtual void CloseConnection(uint16_t cid) = 0;
 
   virtual void SetReadDataReadyCallback(uint16_t cid, ReadDataReadyCallback on_data_ready) = 0;
   virtual void SetConnectionClosedCallback(uint16_t cid, ConnectionClosedCallback on_closed) = 0;
 
-  virtual bool Write(uint16_t cid, const uint8_t* data, size_t len) = 0;
-  virtual bool WriteFlushable(uint16_t cid, const uint8_t* data, size_t len) = 0;
-  virtual bool WriteNonFlushable(uint16_t cid, const uint8_t* data, size_t len) = 0;
+  virtual void Write(uint16_t cid, const uint8_t* data, size_t len) = 0;
+  virtual void WriteFlushable(uint16_t cid, const uint8_t* data, size_t len) = 0;
+  virtual void WriteNonFlushable(uint16_t cid, const uint8_t* data, size_t len) = 0;
 
-  virtual bool IsCongested(uint16_t cid) = 0;
+  virtual void SendLoopbackResponse(std::function<void()>) = 0;
   virtual ~IL2cap() {}
 };
 
diff --git a/gd/shim/l2cap.cc b/gd/shim/l2cap.cc
index 118b1cf..6177272 100644
--- a/gd/shim/l2cap.cc
+++ b/gd/shim/l2cap.cc
@@ -163,28 +163,46 @@
 
   bool Write(ConnectionInterfaceDescriptor cid, std::unique_ptr<packet::RawBuilder> packet);
 
-  void SetHandler(os::Handler* handler) {
-    handler_ = handler;
-  }
-
   size_t NumberOfActiveConnections() const {
     return cid_to_interface_map_.size();
   }
 
-  ConnectionInterfaceManager();
+  void GeneralCallback(ConnectionOpenCallback on_open, hci::Address address, l2cap::Psm psm,
+                       ConnectionInterfaceDescriptor cid) {
+    on_open(address.ToString(), static_cast<uint16_t>(psm), static_cast<uint16_t>(cid));
+  }
+
+  void ConnectionOpened(ConnectionOpenCallback on_open, hci::Address address, l2cap::Psm psm,
+                        ConnectionInterfaceDescriptor cid) {
+    LOG_DEBUG("%s address:%s psm:%hd cid:%hd", __func__, address.ToString().c_str(), psm, cid);
+    handler_->Post(common::BindOnce(&ConnectionInterfaceManager::GeneralCallback, common::Unretained(this), on_open,
+                                    address, psm, cid));
+    // TODO(cmanton) queue this pending connection address/psm tuple up for deletion
+    // There may be multiple, so only remove one
+  }
+
+  void ConnectionFailed(hci::Address address, l2cap::Psm psm) {
+    LOG_DEBUG("%s Connection Failed", __func__);
+    // TODO(cmanton) queue this pending connection address/psm tuple up for deletion
+    // There may be multiple, so only remove one
+  }
+
+  ConnectionInterfaceManager(os::Handler* handler);
 
  private:
-  std::unordered_map<ConnectionInterfaceDescriptor, std::unique_ptr<ConnectionInterface>> cid_to_interface_map_;
-  ConnectionInterfaceDescriptor current_connection_interface_descriptor_;
   os::Handler* handler_;
+  ConnectionInterfaceDescriptor current_connection_interface_descriptor_;
 
   bool HasResources() const;
   bool Exists(ConnectionInterfaceDescriptor id) const;
+
+  std::unordered_map<ConnectionInterfaceDescriptor, std::unique_ptr<ConnectionInterface>> cid_to_interface_map_;
   ConnectionInterfaceDescriptor AllocateConnectionInterfaceDescriptor();
+  ConnectionInterfaceManager() = delete;
 };
 
-ConnectionInterfaceManager::ConnectionInterfaceManager()
-    : current_connection_interface_descriptor_(kStartConnectionInterfaceDescriptor) {}
+ConnectionInterfaceManager::ConnectionInterfaceManager(os::Handler* handler)
+    : handler_(handler), current_connection_interface_descriptor_(kStartConnectionInterfaceDescriptor) {}
 
 bool ConnectionInterfaceManager::Exists(ConnectionInterfaceDescriptor cid) const {
   return cid_to_interface_map_.find(cid) != cid_to_interface_map_.end();
@@ -242,11 +260,58 @@
   return true;
 }
 
+class PendingConnection {
+ public:
+  PendingConnection(ConnectionInterfaceManager* connection_interface_manager, l2cap::Psm psm, hci::Address address,
+                    ConnectionOpenCallback on_open, std::promise<uint16_t> completed)
+      : connection_interface_manager_(connection_interface_manager), psm_(psm), address_(address),
+        on_open_(std::move(on_open)), completed_(std::move(completed)) {}
+
+  void OnConnectionOpen(std::unique_ptr<l2cap::classic::DynamicChannel> channel) {
+    LOG_DEBUG("Local initiated connection is open to device:%s for psm:%hd", address_.ToString().c_str(), psm_);
+    ConnectionInterfaceDescriptor cid = connection_interface_manager_->AddChannel(std::move(channel));
+    completed_.set_value(cid);
+    // Attempt to avoid async race condition with upper stack
+    std::this_thread::yield();
+    connection_interface_manager_->ConnectionOpened(std::move(on_open_), address_, psm_, cid);
+  }
+
+  void OnConnectionFailure(l2cap::classic::DynamicChannelManager::ConnectionResult result) {
+    LOG_DEBUG("Connection failed to device:%s for psm:%hd", address_.ToString().c_str(), psm_);
+    switch (result.connection_result_code) {
+      case l2cap::classic::DynamicChannelManager::ConnectionResultCode::SUCCESS:
+        LOG_WARN("Connection failed result:success hci:%s", hci::ErrorCodeText(result.hci_error).c_str());
+        break;
+      case l2cap::classic::DynamicChannelManager::ConnectionResultCode::FAIL_NO_SERVICE_REGISTERED:
+        LOG_DEBUG("Connection failed result:no service registered hci:%s",
+                  hci::ErrorCodeText(result.hci_error).c_str());
+        break;
+      case l2cap::classic::DynamicChannelManager::ConnectionResultCode::FAIL_HCI_ERROR:
+        LOG_DEBUG("Connection failed result:hci error hci:%s", hci::ErrorCodeText(result.hci_error).c_str());
+        break;
+      case l2cap::classic::DynamicChannelManager::ConnectionResultCode::FAIL_L2CAP_ERROR:
+        LOG_DEBUG("Connection failed result:l2cap error hci:%s l2cap:%s", hci::ErrorCodeText(result.hci_error).c_str(),
+                  l2cap::ConnectionResponseResultText(result.l2cap_connection_response_result).c_str());
+        break;
+    }
+    completed_.set_value(kInvalidConnectionInterfaceDescriptor);
+    connection_interface_manager_->ConnectionFailed(address_, psm_);
+  }
+
+ private:
+  ConnectionInterfaceManager* connection_interface_manager_;
+  const l2cap::Psm psm_;
+  const hci::Address address_;
+  ConnectionOpenCallback on_open_;
+  std::promise<uint16_t> completed_;
+};
+
 class ServiceInterface {
  public:
-  ServiceInterface(uint16_t psm, ServiceInterfaceCallback register_callback,
-                   ConnectionInterfaceCallback connection_callback)
-      : psm_(psm), register_callback_(register_callback), connection_callback_(connection_callback) {}
+  ServiceInterface(ConnectionInterfaceManager* connection_interface_manager, l2cap::Psm psm,
+                   ConnectionOpenCallback on_open, std::promise<void> completed)
+      : connection_interface_manager_(connection_interface_manager), psm_(psm), on_open_(on_open),
+        completed_(std::move(completed)) {}
 
   void OnRegistrationComplete(l2cap::classic::DynamicChannelManager::RegistrationResult result,
                               std::unique_ptr<l2cap::classic::DynamicChannelService> service) {
@@ -254,12 +319,15 @@
     ASSERT(psm_ == service->GetPsm());
     LOG_DEBUG("Registration is complete for psm:%hd", psm_);
     service_ = std::move(service);
-    register_callback_(psm_, result);
+    completed_.set_value();
   }
 
   void OnConnectionOpen(std::unique_ptr<l2cap::classic::DynamicChannel> channel) {
-    LOG_DEBUG("Connection is open to device:%s for psm:%hd", channel->GetDevice().ToString().c_str(), psm_);
-    connection_callback_(psm_, std::move(channel));
+    LOG_DEBUG("Remote initiated connection is open from device:%s for psm:%hd", channel->GetDevice().ToString().c_str(),
+              psm_);
+    hci::Address address = channel->GetDevice();
+    ConnectionInterfaceDescriptor cid = connection_interface_manager_->AddChannel(std::move(channel));
+    connection_interface_manager_->ConnectionOpened(on_open_, address, psm_, cid);
   }
 
   l2cap::SecurityPolicy GetSecurityPolicy() const {
@@ -276,133 +344,59 @@
   }
 
  private:
+  ConnectionInterfaceManager* connection_interface_manager_;
   const l2cap::Psm psm_;
+  ConnectionOpenCallback on_open_;
+  std::promise<void> completed_;
+
   std::unique_ptr<l2cap::classic::DynamicChannelService> service_;
+
   const l2cap::SecurityPolicy security_policy_;
-  ServiceInterfaceCallback register_callback_;
-  ConnectionInterfaceCallback connection_callback_;
 };
 
 struct L2cap::impl {
   void RegisterService(l2cap::Psm psm, ConnectionOpenCallback on_open, std::promise<void> completed);
   void UnregisterService(l2cap::Psm psm);
 
-  void CreateConnection(l2cap::Psm psm, hci::Address address, std::promise<uint16_t> completed);
+  void CreateConnection(l2cap::Psm psm, hci::Address address, ConnectionOpenCallback on_open,
+                        std::promise<uint16_t> completed);
   void CloseConnection(ConnectionInterfaceDescriptor cid);
 
-  void OnConnectionOpenNever(std::unique_ptr<l2cap::classic::DynamicChannel> channel);
-  void OnConnectionFailureNever(l2cap::classic::DynamicChannelManager::ConnectionResult result);
+  void SetReadDataReadyCallback(ConnectionInterfaceDescriptor cid, ReadDataReadyCallback on_data_ready);
+  void SetConnectionClosedCallback(ConnectionInterfaceDescriptor cid, ConnectionClosedCallback on_closed);
 
-  bool Write(ConnectionInterfaceDescriptor cid, std::unique_ptr<packet::RawBuilder> packet);
+  void Write(ConnectionInterfaceDescriptor cid, std::unique_ptr<packet::RawBuilder> packet);
+
+  void SendLoopbackResponse(std::function<void()> function);
 
   impl(L2cap& module, l2cap::classic::L2capClassicModule* l2cap_module);
-  ConnectionInterfaceManager connection_interface_manager_;
-
-  void OpenConnection(l2cap::Psm psm, ConnectionInterfaceDescriptor cid);
 
  private:
   L2cap& module_;
-  l2cap::classic::L2capClassicModule* l2cap_module_{nullptr};
+  l2cap::classic::L2capClassicModule* l2cap_module_;
+  os::Handler* handler_;
+  ConnectionInterfaceManager connection_interface_manager_;
+
   std::unique_ptr<l2cap::classic::DynamicChannelManager> dynamic_channel_manager_;
 
   std::unordered_map<l2cap::Psm, std::shared_ptr<ServiceInterface>> psm_to_service_interface_map_;
-  std::unordered_map<l2cap::Psm, ConnectionOpenCallback> psm_to_on_open_map_;
-
-  std::mutex mutex_;
-  std::unordered_map<l2cap::Psm, std::promise<void>> psm_to_register_complete_map_;
-  std::unordered_map<l2cap::Psm, std::queue<std::promise<uint16_t>>> psm_to_connect_completed_queue_;
-
-  os::Handler* handler_;
+  std::unordered_map<l2cap::Psm, std::shared_ptr<PendingConnection>> psm_to_pending_connection_map_;
 };
 
 L2cap::impl::impl(L2cap& module, l2cap::classic::L2capClassicModule* l2cap_module)
-    : module_(module), l2cap_module_(l2cap_module) {
-  handler_ = module_.GetHandler();
+    : module_(module), l2cap_module_(l2cap_module), handler_(module_.GetHandler()),
+      connection_interface_manager_(handler_) {
   dynamic_channel_manager_ = l2cap_module_->GetDynamicChannelManager();
-  connection_interface_manager_.SetHandler(handler_);
-}
-
-void L2cap::impl::OnConnectionOpenNever(std::unique_ptr<l2cap::classic::DynamicChannel> channel) {
-  ASSERT(false);
-}
-
-void L2cap::impl::OnConnectionFailureNever(l2cap::classic::DynamicChannelManager::ConnectionResult result) {
-  ASSERT(false);
-  switch (result.connection_result_code) {
-    case l2cap::classic::DynamicChannelManager::ConnectionResultCode::SUCCESS:
-      LOG_WARN("Connection failed result:success hci:%s", hci::ErrorCodeText(result.hci_error).c_str());
-      break;
-    case l2cap::classic::DynamicChannelManager::ConnectionResultCode::FAIL_NO_SERVICE_REGISTERED:
-      LOG_DEBUG("Connection failed result:no service registered hci:%s", hci::ErrorCodeText(result.hci_error).c_str());
-      break;
-    case l2cap::classic::DynamicChannelManager::ConnectionResultCode::FAIL_HCI_ERROR:
-      LOG_DEBUG("Connection failed result:hci error hci:%s", hci::ErrorCodeText(result.hci_error).c_str());
-      break;
-    case l2cap::classic::DynamicChannelManager::ConnectionResultCode::FAIL_L2CAP_ERROR:
-      LOG_DEBUG("Connection failed result:l2cap error hci:%s l2cap:%s", hci::ErrorCodeText(result.hci_error).c_str(),
-                l2cap::ConnectionResponseResultText(result.l2cap_connection_response_result).c_str());
-      break;
-  }
-}
-
-void L2cap::impl::OpenConnection(l2cap::Psm psm, ConnectionInterfaceDescriptor cid) {
-  LOG_INFO("About to call back to client indicating open connection psm:%hd cid:%hd", psm, cid);
-  psm_to_on_open_map_[psm](psm, cid, [cid](std::function<void(uint16_t cid)> func) {
-    LOG_DEBUG("About to run postable on this thread and inform sdp that connection is open");
-    func(cid);
-  });
 }
 
 void L2cap::impl::RegisterService(l2cap::Psm psm, ConnectionOpenCallback on_open, std::promise<void> completed) {
   ASSERT(psm_to_service_interface_map_.find(psm) == psm_to_service_interface_map_.end());
-  ASSERT(psm_to_register_complete_map_.find(psm) == psm_to_register_complete_map_.end());
-  ASSERT(psm_to_on_open_map_.find(psm) == psm_to_on_open_map_.end());
 
-  {
-    std::unique_lock<std::mutex> lock(mutex_);
-    psm_to_on_open_map_[psm] = on_open;
-    psm_to_register_complete_map_[psm] = std::move(completed);
-  }
+  auto service_interface =
+      std::make_shared<ServiceInterface>(&connection_interface_manager_, psm, on_open, std::move(completed));
+  psm_to_service_interface_map_.emplace(psm, service_interface);
 
-  psm_to_service_interface_map_.emplace(
-      psm, std::make_shared<ServiceInterface>(
-               psm,
-               [this](l2cap::Psm psm, l2cap::classic::DynamicChannelManager::RegistrationResult result) {
-                 LOG_DEBUG("Service has been registered");
-                 ASSERT(psm_to_register_complete_map_.find(psm) != psm_to_register_complete_map_.end());
-                 {
-                   std::unique_lock<std::mutex> lock(mutex_);
-                   auto completed = std::move(psm_to_register_complete_map_[psm]);
-                   psm_to_register_complete_map_.erase(psm);
-                   completed.set_value();
-                 }
-               },
-
-               [this](l2cap::Psm psm, std::unique_ptr<l2cap::classic::DynamicChannel> channel) {
-                 ConnectionInterfaceDescriptor cid = connection_interface_manager_.AddChannel(std::move(channel));
-                 LOG_DEBUG("Connection has been opened cid:%hd psm:%hd", cid, psm);
-                 {
-                   // If initiated locally unblock requestor that
-                   // we now have a connection by providing the
-                   // cid.
-                   std::unique_lock<std::mutex> lock(mutex_);
-                   if (psm_to_connect_completed_queue_.find(psm) != psm_to_connect_completed_queue_.end()) {
-                     if (!psm_to_connect_completed_queue_[psm].empty()) {
-                       LOG_DEBUG("Locally initiated, so inform waiting client of the cid %hd", cid);
-                       auto completed = std::move(psm_to_connect_completed_queue_[psm].front());
-                       psm_to_connect_completed_queue_[psm].pop();
-                       completed.set_value(cid);
-                     }
-                   }
-                   std::this_thread::yield();
-                 }
-                 if (cid != kInvalidConnectionInterfaceDescriptor) {
-                   handler_->Post(common::BindOnce(&L2cap::impl::OpenConnection, common::Unretained(this), psm, cid));
-                 }
-                 usleep(10);
-               }));
-
-  psm_to_service_interface_map_.find(psm)->second->RegisterService(
+  service_interface->RegisterService(
       [this](l2cap::Psm psm, l2cap::SecurityPolicy security_policy,
              l2cap::classic::DynamicChannelManager::OnRegistrationCompleteCallback on_registration_complete,
              l2cap::classic::DynamicChannelManager::OnConnectionOpenCallback on_connection_open) {
@@ -416,24 +410,40 @@
   psm_to_service_interface_map_.erase(psm);
 }
 
-void L2cap::impl::CreateConnection(l2cap::Psm psm, hci::Address address, std::promise<uint16_t> completed) {
-  LOG_INFO("Creating connection to psm:%hd device:%s", psm, address.ToString().c_str());
-  {
-    std::unique_lock<std::mutex> lock(mutex_);
-    psm_to_connect_completed_queue_[psm].push(std::move(completed));
-  }
+void L2cap::impl::CreateConnection(l2cap::Psm psm, hci::Address address, ConnectionOpenCallback on_open,
+                                   std::promise<uint16_t> completed) {
+  LOG_DEBUG("Initiating classic connection to psm:%hd device:%s", psm, address.ToString().c_str());
+  auto pending_connection = std::make_shared<PendingConnection>(&connection_interface_manager_, psm, address,
+                                                                std::move(on_open), std::move(completed));
+  // TODO(cmanton) hash psm/address pair into unordered map for pending_connection
+  // This is ok for now
+  psm_to_pending_connection_map_[psm] = pending_connection;
+
   bool rc = dynamic_channel_manager_->ConnectChannel(
-      address, psm, common::Bind(&L2cap::impl::OnConnectionOpenNever, common::Unretained(this)),
-      common::Bind(&L2cap::impl::OnConnectionFailureNever, common::Unretained(this)), handler_);
-  ASSERT_LOG(rc == true, "Failed to create classic connection channel");
+      address, psm, common::Bind(&PendingConnection::OnConnectionOpen, common::Unretained(pending_connection.get())),
+      common::BindOnce(&PendingConnection::OnConnectionFailure, common::Unretained(pending_connection.get())),
+      handler_);
+  ASSERT_LOG(rc == true, "Failed to create classic connection");
 }
 
 void L2cap::impl::CloseConnection(ConnectionInterfaceDescriptor cid) {
   connection_interface_manager_.RemoveConnection(cid);
 }
 
-bool L2cap::impl::Write(ConnectionInterfaceDescriptor cid, std::unique_ptr<packet::RawBuilder> packet) {
-  return connection_interface_manager_.Write(cid, std::move(packet));
+void L2cap::impl::SetReadDataReadyCallback(ConnectionInterfaceDescriptor cid, ReadDataReadyCallback on_data_ready) {
+  connection_interface_manager_.SetReadDataReadyCallback(cid, on_data_ready);
+}
+
+void L2cap::impl::SetConnectionClosedCallback(ConnectionInterfaceDescriptor cid, ConnectionClosedCallback on_closed) {
+  connection_interface_manager_.SetConnectionClosedCallback(cid, std::move(on_closed));
+}
+
+void L2cap::impl::Write(ConnectionInterfaceDescriptor cid, std::unique_ptr<packet::RawBuilder> packet) {
+  connection_interface_manager_.Write(cid, std::move(packet));
+}
+
+void L2cap::impl::SendLoopbackResponse(std::function<void()> function) {
+  function();
 }
 
 void L2cap::RegisterService(uint16_t raw_psm, ConnectionOpenCallback on_open, std::promise<void> completed) {
@@ -444,51 +454,54 @@
 
 void L2cap::UnregisterService(uint16_t raw_psm) {
   l2cap::Psm psm{raw_psm};
-  pimpl_->UnregisterService(psm);
+  GetHandler()->Post(common::Bind(&L2cap::impl::UnregisterService, common::Unretained(pimpl_.get()), psm));
 }
 
-void L2cap::CreateConnection(uint16_t raw_psm, const std::string address_string, std::promise<uint16_t> completed) {
+void L2cap::CreateConnection(uint16_t raw_psm, const std::string address_string, ConnectionOpenCallback on_open,
+                             std::promise<uint16_t> completed) {
   l2cap::Psm psm{raw_psm};
   hci::Address address;
   hci::Address::FromString(address_string, address);
 
   GetHandler()->Post(common::BindOnce(&L2cap::impl::CreateConnection, common::Unretained(pimpl_.get()), psm, address,
-                                      std::move(completed)));
+                                      on_open, std::move(completed)));
 }
 
 void L2cap::CloseConnection(uint16_t raw_cid) {
   ConnectionInterfaceDescriptor cid(raw_cid);
-  return pimpl_->CloseConnection(cid);
+  GetHandler()->Post(common::Bind(&L2cap::impl::CloseConnection, common::Unretained(pimpl_.get()), cid));
 }
 
-void L2cap::SetReadDataReadyCallback(uint16_t cid, ReadDataReadyCallback on_data_ready) {
-  pimpl_->connection_interface_manager_.SetReadDataReadyCallback(static_cast<ConnectionInterfaceDescriptor>(cid),
-                                                                 on_data_ready);
+void L2cap::SetReadDataReadyCallback(uint16_t raw_cid, ReadDataReadyCallback on_data_ready) {
+  ConnectionInterfaceDescriptor cid(raw_cid);
+  GetHandler()->Post(
+      common::Bind(&L2cap::impl::SetReadDataReadyCallback, common::Unretained(pimpl_.get()), cid, on_data_ready));
 }
 
-void L2cap::SetConnectionClosedCallback(uint16_t cid, ConnectionClosedCallback on_closed) {
-  pimpl_->connection_interface_manager_.SetConnectionClosedCallback(static_cast<ConnectionInterfaceDescriptor>(cid),
-                                                                    on_closed);
+void L2cap::SetConnectionClosedCallback(uint16_t raw_cid, ConnectionClosedCallback on_closed) {
+  ConnectionInterfaceDescriptor cid(raw_cid);
+  GetHandler()->Post(common::Bind(&L2cap::impl::SetConnectionClosedCallback, common::Unretained(pimpl_.get()), cid,
+                                  std::move(on_closed)));
 }
 
-bool L2cap::Write(uint16_t cid, const uint8_t* data, size_t len) {
+void L2cap::Write(uint16_t raw_cid, const uint8_t* data, size_t len) {
+  ConnectionInterfaceDescriptor cid(raw_cid);
   auto packet = MakeUniquePacket(data, len);
-  return pimpl_->Write(static_cast<ConnectionInterfaceDescriptor>(cid), std::move(packet));
+  GetHandler()->Post(common::BindOnce(&L2cap::impl::Write, common::Unretained(pimpl_.get()), cid, std::move(packet)));
 }
 
-bool L2cap::WriteFlushable(uint16_t cid, const uint8_t* data, size_t len) {
+void L2cap::WriteFlushable(uint16_t raw_cid, const uint8_t* data, size_t len) {
   LOG_WARN("UNIMPLEMENTED Write flushable");
-  return false;
+  return Write(raw_cid, data, len);
 }
 
-bool L2cap::WriteNonFlushable(uint16_t cid, const uint8_t* data, size_t len) {
+void L2cap::WriteNonFlushable(uint16_t raw_cid, const uint8_t* data, size_t len) {
   LOG_WARN("UNIMPLEMENTED Write non flushable");
-  return false;
+  return Write(raw_cid, data, len);
 }
 
-bool L2cap::IsCongested(ConnectionInterfaceDescriptor cid) {
-  LOG_WARN("UNIMPLEMENTED Congestion check on channels or links");
-  return false;
+void L2cap::SendLoopbackResponse(std::function<void()> function) {
+  GetHandler()->Post(common::BindOnce(&L2cap::impl::SendLoopbackResponse, common::Unretained(pimpl_.get()), function));
 }
 
 /**
diff --git a/gd/shim/l2cap.h b/gd/shim/l2cap.h
index 67a75d9..955d20c 100644
--- a/gd/shim/l2cap.h
+++ b/gd/shim/l2cap.h
@@ -32,17 +32,18 @@
   void RegisterService(uint16_t psm, ConnectionOpenCallback on_open, std::promise<void> completed) override;
   void UnregisterService(uint16_t psm) override;
 
-  void CreateConnection(uint16_t psm, const std::string address, std::promise<uint16_t> completed) override;
+  void CreateConnection(uint16_t psm, const std::string address_string, ConnectionOpenCallback on_open,
+                        std::promise<uint16_t> completed) override;
   void CloseConnection(uint16_t cid) override;
 
   void SetReadDataReadyCallback(uint16_t cid, ReadDataReadyCallback on_data_ready) override;
   void SetConnectionClosedCallback(uint16_t cid, ConnectionClosedCallback on_closed) override;
 
-  bool Write(uint16_t cid, const uint8_t* data, size_t len) override;
-  bool WriteFlushable(uint16_t cid, const uint8_t* data, size_t len) override;
-  bool WriteNonFlushable(uint16_t cid, const uint8_t* data, size_t len) override;
+  void Write(uint16_t cid, const uint8_t* data, size_t len) override;
+  void WriteFlushable(uint16_t cid, const uint8_t* data, size_t len) override;
+  void WriteNonFlushable(uint16_t cid, const uint8_t* data, size_t len) override;
 
-  bool IsCongested(uint16_t cid) override;
+  void SendLoopbackResponse(std::function<void()>) override;
 
   L2cap() = default;
   ~L2cap() = default;
diff --git a/main/shim/l2cap.cc b/main/shim/l2cap.cc
index 4fa6127..fb4dd02 100644
--- a/main/shim/l2cap.cc
+++ b/main/shim/l2cap.cc
@@ -193,8 +193,12 @@
 
   std::promise<uint16_t> connect_completed;
   auto completed = connect_completed.get_future();
-  bluetooth::shim::GetL2cap()->CreateConnection(psm, raw_address.ToString(),
-                                                std::move(connect_completed));
+  bluetooth::shim::GetL2cap()->CreateConnection(
+      psm, raw_address.ToString(),
+      std::bind(&bluetooth::legacy::shim::L2cap::OnConnectionReady, this,
+                std::placeholders::_1, std::placeholders::_2,
+                std::placeholders::_3),
+      std::move(connect_completed));
   uint16_t cid = completed.get();
   if (cid == kInvalidConnectionInterfaceDescriptor) {
     LOG_WARN(LOG_TAG,
@@ -215,8 +219,7 @@
 }
 
 void bluetooth::legacy::shim::L2cap::OnConnectionReady(
-    uint16_t psm, uint16_t cid,
-    std::function<void(std::function<void(uint16_t c)>)> func) {
+    std::string address_string, uint16_t psm, uint16_t cid) {
   LOG_DEBUG(
       LOG_TAG,
       "l2cap got new connection psm:%hd connection_interface_descriptor:%hd",
@@ -226,14 +229,6 @@
     return;
   }
   LOG_DEBUG(LOG_TAG, "%s Setting postable map for cid:%d", __func__, cid);
-  cid_to_postable_map_[cid] = func;
-  func([&cid, &callbacks](uint16_t cid2) {
-    LOG_WARN(LOG_TAG,
-             "Queuing up the connection confirm to the upper stack but really "
-             "a connection has already been done Cid:%hd Cid2:%hd",
-             cid, cid2);
-    callbacks->pL2CA_ConnectCfm_Cb(cid2, 0);
-  });
 }
 
 bool bluetooth::legacy::shim::L2cap::Write(uint16_t cid, BT_HDR* bt_hdr) {
@@ -242,7 +237,8 @@
   const uint8_t* data = bt_hdr->data + bt_hdr->offset;
   size_t len = bt_hdr->len;
   LOG_DEBUG(LOG_TAG, "Writing data cid:%hd len:%zd", cid, len);
-  return bluetooth::shim::GetL2cap()->Write(cid, data, len);
+  bluetooth::shim::GetL2cap()->Write(cid, data, len);
+  return true;
 }
 
 bool bluetooth::legacy::shim::L2cap::WriteFlushable(uint16_t cid,
@@ -251,7 +247,8 @@
   CHECK(bt_hdr != nullptr);
   const uint8_t* data = bt_hdr->data + bt_hdr->offset;
   size_t len = bt_hdr->len;
-  return bluetooth::shim::GetL2cap()->WriteFlushable(cid, data, len);
+  bluetooth::shim::GetL2cap()->WriteFlushable(cid, data, len);
+  return true;
 }
 
 bool bluetooth::legacy::shim::L2cap::WriteNonFlushable(uint16_t cid,
@@ -260,7 +257,8 @@
   CHECK(bt_hdr != nullptr);
   const uint8_t* data = bt_hdr->data + bt_hdr->offset;
   size_t len = bt_hdr->len;
-  return bluetooth::shim::GetL2cap()->WriteNonFlushable(cid, data, len);
+  bluetooth::shim::GetL2cap()->WriteNonFlushable(cid, data, len);
+  return true;
 }
 
 bool bluetooth::legacy::shim::L2cap::SetCallbacks(
diff --git a/main/shim/l2cap.h b/main/shim/l2cap.h
index da9852a..e0828d6 100644
--- a/main/shim/l2cap.h
+++ b/main/shim/l2cap.h
@@ -54,9 +54,8 @@
                        bool enable_snoop);
   void UnregisterService(uint16_t psm);
   uint16_t CreateConnection(uint16_t psm, const RawAddress& raw_address);
-  void OnConnectionReady(
-      uint16_t psm, uint16_t cid,
-      std::function<void(std::function<void(uint16_t cid)>)> func);
+  void OnConnectionReady(std::string address_string, uint16_t psm,
+                         uint16_t cid);
 
   bool Write(uint16_t cid, BT_HDR* bt_hdr);
   bool WriteFlushable(uint16_t cid, BT_HDR* bt_hdr);
diff --git a/main/shim/l2cap_test.cc b/main/shim/l2cap_test.cc
index 1f67de6..a7dddde 100644
--- a/main/shim/l2cap_test.cc
+++ b/main/shim/l2cap_test.cc
@@ -232,14 +232,8 @@
   uint16_t cid = l2cap_->CreateConnection(kPsm, raw_address);
   CHECK(cid != 0);
 
-  {
-    // Simulate a successful connection response
-    l2cap_->OnConnectionReady(kPsm, kCid,
-                              [&cid](std::function<void(uint16_t)> func) {
-                                LOG_INFO(LOG_TAG, "In closure cid:%d", cid);
-                                func(cid);
-                              });
-  }
+  // Simulate a successful connection response
+  l2cap_->OnConnectionReady("11:22:33:44:55:66", kPsm, kCid);
   CHECK(l2cap_->ConfigRequest(cid, nullptr));
 }
 
@@ -253,14 +247,8 @@
   uint16_t cid = l2cap_->CreateConnection(kPsm, raw_address);
   CHECK(cid != 0);
 
-  {
-    // Simulate a successful connection response
-    l2cap_->OnConnectionReady(kPsm, kCid,
-                              [&cid](std::function<void(uint16_t)> func) {
-                                LOG_INFO(LOG_TAG, "In closure cid:%d", cid);
-                                func(cid);
-                              });
-  }
+  // Simulate a successful connection response
+  l2cap_->OnConnectionReady("11:22:33:44:55:66", kPsm, kCid);
   CHECK(l2cap_->ConfigResponse(cid, nullptr));
 }
 
@@ -274,14 +262,8 @@
   uint16_t cid = l2cap_->CreateConnection(kPsm, raw_address);
   CHECK(cid != 0);
 
-  {
-    // Simulate a successful connection response
-    l2cap_->OnConnectionReady(kPsm, kCid,
-                              [&cid](std::function<void(uint16_t)> func) {
-                                LOG_INFO(LOG_TAG, "In closure cid:%d", cid);
-                                func(cid);
-                              });
-  }
+  // Simulate a successful connection response
+  l2cap_->OnConnectionReady("11:22:33:44:55:66", kPsm, kCid);
   CHECK(l2cap_->DisconnectRequest(cid));
 }
 
@@ -295,14 +277,8 @@
   uint16_t cid = l2cap_->CreateConnection(kPsm, raw_address);
   CHECK(cid != 0);
 
-  {
-    // Simulate a successful connection response
-    l2cap_->OnConnectionReady(kPsm, kCid,
-                              [&cid](std::function<void(uint16_t)> func) {
-                                LOG_INFO(LOG_TAG, "In closure cid:%d", cid);
-                                func(cid);
-                              });
-  }
+  // Simulate a successful connection response
+  l2cap_->OnConnectionReady("11:22:33:44:55:66", kPsm, kCid);
   CHECK(l2cap_->DisconnectResponse(cid));
 }
 
@@ -316,14 +292,8 @@
   uint16_t cid = l2cap_->CreateConnection(kPsm, raw_address);
   CHECK(cid != 0);
 
-  {
-    // Simulate a successful connection response
-    l2cap_->OnConnectionReady(kPsm, kCid,
-                              [&cid](std::function<void(uint16_t)> func) {
-                                LOG_INFO(LOG_TAG, "In closure cid:%d", cid);
-                                func(cid);
-                              });
-  }
+  // Simulate a successful connection response
+  l2cap_->OnConnectionReady("11:22:33:44:55:66", kPsm, kCid);
   CHECK(cnt_.L2caConnectCfmCb == 1);
 
   CHECK(l2cap_->ConfigRequest(cid, nullptr) == true);
diff --git a/main/shim/test_stack.cc b/main/shim/test_stack.cc
index e475d49..5d4ede5 100644
--- a/main/shim/test_stack.cc
+++ b/main/shim/test_stack.cc
@@ -39,8 +39,10 @@
   registered_service_.erase(psm);
 }
 
-void TestGdShimL2cap::CreateConnection(uint16_t psm, const std::string address,
-                                       std::promise<uint16_t> completed) {
+void TestGdShimL2cap::CreateConnection(
+    uint16_t psm, const std::string address,
+    bluetooth::shim::ConnectionOpenCallback on_open,
+    std::promise<uint16_t> completed) {
   completed.set_value(cid_);
 }
 
@@ -52,24 +54,19 @@
 void TestGdShimL2cap::SetConnectionClosedCallback(
     uint16_t cid, bluetooth::shim::ConnectionClosedCallback on_closed) {}
 
-bool TestGdShimL2cap::Write(uint16_t cid, const uint8_t* data, size_t len) {
+void TestGdShimL2cap::Write(uint16_t cid, const uint8_t* data, size_t len) {
   ASSERT(data_buffer_ != nullptr);
   ASSERT(data_buffer_size_ > len);
   memcpy(data_buffer_, data, len);
-  return write_success_;
 }
 
-bool TestGdShimL2cap::WriteFlushable(uint16_t cid, const uint8_t* data,
-                                     size_t len) {
-  return write_success_;
-}
+void TestGdShimL2cap::WriteFlushable(uint16_t cid, const uint8_t* data,
+                                     size_t len) {}
 
-bool TestGdShimL2cap::WriteNonFlushable(uint16_t cid, const uint8_t* data,
-                                        size_t len) {
-  return write_success_;
-}
+void TestGdShimL2cap::WriteNonFlushable(uint16_t cid, const uint8_t* data,
+                                        size_t len) {}
 
-bool TestGdShimL2cap::IsCongested(uint16_t cid) { return is_congested_; }
+void TestGdShimL2cap::SendLoopbackResponse(std::function<void()>) {}
 
 void TestStack::Start() {}
 
diff --git a/main/shim/test_stack.h b/main/shim/test_stack.h
index e8bdc13..ee0986a 100644
--- a/main/shim/test_stack.h
+++ b/main/shim/test_stack.h
@@ -34,6 +34,7 @@
                        std::promise<void> completed) override;
   void UnregisterService(uint16_t psm);
   void CreateConnection(uint16_t psm, const std::string address,
+                        bluetooth::shim::ConnectionOpenCallback on_open,
                         std::promise<uint16_t> completed) override;
   void CloseConnection(uint16_t cid);
   void SetReadDataReadyCallback(
@@ -42,11 +43,11 @@
   void SetConnectionClosedCallback(
       uint16_t cid,
       bluetooth::shim::ConnectionClosedCallback on_closed) override;
-  bool Write(uint16_t cid, const uint8_t* data, size_t len) override;
-  bool WriteFlushable(uint16_t cid, const uint8_t* data, size_t len) override;
-  bool WriteNonFlushable(uint16_t cid, const uint8_t* data,
+  void Write(uint16_t cid, const uint8_t* data, size_t len) override;
+  void WriteFlushable(uint16_t cid, const uint8_t* data, size_t len) override;
+  void WriteNonFlushable(uint16_t cid, const uint8_t* data,
                          size_t len) override;
-  bool IsCongested(uint16_t cid) override;
+  void SendLoopbackResponse(std::function<void()>) override;
 };
 
 class TestStack : public bluetooth::shim::IStack {