diff --git a/wifi/1.0/default/wifi.cpp b/wifi/1.0/default/wifi.cpp
index 19f7e53..30adcc0 100644
--- a/wifi/1.0/default/wifi.cpp
+++ b/wifi/1.0/default/wifi.cpp
@@ -34,6 +34,7 @@
 
 Wifi::Wifi()
     : legacy_hal_(new legacy_hal::WifiLegacyHal()),
+      mode_controller_(new mode_controller::WifiModeController()),
       run_state_(RunState::STOPPED) {}
 
 bool Wifi::isValid() {
@@ -96,25 +97,29 @@
     return createWifiStatus(WifiStatusCode::ERROR_NOT_AVAILABLE,
                             "HAL is stopping");
   }
-
-  LOG(INFO) << "Starting HAL";
-  legacy_hal::wifi_error legacy_status = legacy_hal_->start();
-  if (legacy_status != legacy_hal::WIFI_SUCCESS) {
-    LOG(ERROR) << "Failed to start Wifi HAL: "
-               << legacyErrorToString(legacy_status);
-    return createWifiStatusFromLegacyError(legacy_status,
-                                           "Failed to start HAL");
+  WifiStatus wifi_status = initializeLegacyHal();
+  if (wifi_status.code == WifiStatusCode::SUCCESS) {
+    // Create the chip instance once the HAL is started.
+    chip_ = new WifiChip(kChipId, legacy_hal_, mode_controller_);
+    run_state_ = RunState::STARTED;
+    for (const auto& callback : event_callbacks_) {
+      if (!callback->onStart().getStatus().isOk()) {
+        LOG(ERROR) << "Failed to invoke onStart callback";
+      };
+    }
+    for (const auto& callback : event_callbacks_) {
+      if (!callback->onFailure(wifi_status).getStatus().isOk()) {
+        LOG(ERROR) << "Failed to invoke onFailure callback";
+      }
+    }
+  } else {
+    for (const auto& callback : event_callbacks_) {
+      if (!callback->onFailure(wifi_status).getStatus().isOk()) {
+        LOG(ERROR) << "Failed to invoke onFailure callback";
+      }
+    }
   }
-
-  // Create the chip instance once the HAL is started.
-  chip_ = new WifiChip(kChipId, legacy_hal_);
-  run_state_ = RunState::STARTED;
-  for (const auto& callback : event_callbacks_) {
-    if (!callback->onStart().getStatus().isOk()) {
-      LOG(ERROR) << "Failed to invoke onStart callback";
-    };
-  }
-  return createWifiStatus(WifiStatusCode::SUCCESS);
+  return wifi_status;
 }
 
 WifiStatus Wifi::stopInternal() {
@@ -124,34 +129,21 @@
     return createWifiStatus(WifiStatusCode::ERROR_NOT_AVAILABLE,
                             "HAL is stopping");
   }
-
-  LOG(INFO) << "Stopping HAL";
-  run_state_ = RunState::STOPPING;
-  const auto on_complete_callback_ = [&]() {
-    if (chip_.get()) {
-      chip_->invalidate();
-    }
-    chip_.clear();
-    run_state_ = RunState::STOPPED;
+  WifiStatus wifi_status = stopLegacyHalAndDeinitializeModeController();
+  if (wifi_status.code == WifiStatusCode::SUCCESS) {
     for (const auto& callback : event_callbacks_) {
       if (!callback->onStop().getStatus().isOk()) {
         LOG(ERROR) << "Failed to invoke onStop callback";
       };
     }
-  };
-  legacy_hal::wifi_error legacy_status =
-      legacy_hal_->stop(on_complete_callback_);
-  if (legacy_status != legacy_hal::WIFI_SUCCESS) {
-    LOG(ERROR) << "Failed to stop Wifi HAL: "
-               << legacyErrorToString(legacy_status);
-    WifiStatus wifi_status =
-        createWifiStatusFromLegacyError(legacy_status, "Failed to stop HAL");
+  } else {
     for (const auto& callback : event_callbacks_) {
-      callback->onFailure(wifi_status);
+      if (!callback->onFailure(wifi_status).getStatus().isOk()) {
+        LOG(ERROR) << "Failed to invoke onFailure callback";
+      }
     }
-    return wifi_status;
   }
-  return createWifiStatus(WifiStatusCode::SUCCESS);
+  return wifi_status;
 }
 
 std::pair<WifiStatus, std::vector<ChipId>> Wifi::getChipIdsInternal() {
@@ -171,6 +163,41 @@
   }
   return {createWifiStatus(WifiStatusCode::SUCCESS), chip_};
 }
+
+WifiStatus Wifi::initializeLegacyHal() {
+  LOG(INFO) << "Initializing legacy HAL";
+  legacy_hal::wifi_error legacy_status = legacy_hal_->initialize();
+  if (legacy_status != legacy_hal::WIFI_SUCCESS) {
+    LOG(ERROR) << "Failed to initialize legacy HAL: "
+               << legacyErrorToString(legacy_status);
+    return createWifiStatusFromLegacyError(legacy_status);
+  }
+  return createWifiStatus(WifiStatusCode::SUCCESS);
+}
+
+WifiStatus Wifi::stopLegacyHalAndDeinitializeModeController() {
+  LOG(INFO) << "Stopping legacy HAL";
+  run_state_ = RunState::STOPPING;
+  const auto on_complete_callback_ = [&]() {
+    if (chip_.get()) {
+      chip_->invalidate();
+    }
+    chip_.clear();
+    run_state_ = RunState::STOPPED;
+  };
+  legacy_hal::wifi_error legacy_status =
+      legacy_hal_->stop(on_complete_callback_);
+  if (legacy_status != legacy_hal::WIFI_SUCCESS) {
+    LOG(ERROR) << "Failed to stop legacy HAL: "
+               << legacyErrorToString(legacy_status);
+    return createWifiStatusFromLegacyError(legacy_status);
+  }
+  if (!mode_controller_->deinitialize()) {
+    LOG(ERROR) << "Failed to deinitialize firmware mode controller";
+    return createWifiStatus(WifiStatusCode::ERROR_UNKNOWN);
+  }
+  return createWifiStatus(WifiStatusCode::SUCCESS);
+}
 }  // namespace implementation
 }  // namespace V1_0
 }  // namespace wifi
diff --git a/wifi/1.0/default/wifi.h b/wifi/1.0/default/wifi.h
index 7872303..40d3552 100644
--- a/wifi/1.0/default/wifi.h
+++ b/wifi/1.0/default/wifi.h
@@ -25,6 +25,7 @@
 
 #include "wifi_chip.h"
 #include "wifi_legacy_hal.h"
+#include "wifi_mode_controller.h"
 
 namespace android {
 namespace hardware {
@@ -62,9 +63,13 @@
   std::pair<WifiStatus, std::vector<ChipId>> getChipIdsInternal();
   std::pair<WifiStatus, sp<IWifiChip>> getChipInternal(ChipId chip_id);
 
+  WifiStatus initializeLegacyHal();
+  WifiStatus stopLegacyHalAndDeinitializeModeController();
+
   // Instance is created in this root level |IWifi| HIDL interface object
   // and shared with all the child HIDL interface objects.
   std::shared_ptr<legacy_hal::WifiLegacyHal> legacy_hal_;
+  std::shared_ptr<mode_controller::WifiModeController> mode_controller_;
   RunState run_state_;
   std::vector<sp<IWifiEventCallback>> event_callbacks_;
   sp<WifiChip> chip_;
diff --git a/wifi/1.0/default/wifi_chip.cpp b/wifi/1.0/default/wifi_chip.cpp
index 3ab6052..2487d9f 100644
--- a/wifi/1.0/default/wifi_chip.cpp
+++ b/wifi/1.0/default/wifi_chip.cpp
@@ -24,6 +24,12 @@
 using android::sp;
 using android::hardware::hidl_vec;
 using android::hardware::hidl_string;
+using android::hardware::wifi::V1_0::IWifiChip;
+using android::hardware::wifi::V1_0::IfaceType;
+
+constexpr uint32_t kStaChipModeId = 0;
+constexpr uint32_t kApChipModeId = 1;
+constexpr uint32_t kInvalidModeId = UINT32_MAX;
 
 template <typename Iface>
 void invalidateAndClear(sp<Iface>& iface) {
@@ -41,9 +47,15 @@
 namespace implementation {
 using hidl_return_util::validateAndCall;
 
-WifiChip::WifiChip(ChipId chip_id,
-                   const std::weak_ptr<legacy_hal::WifiLegacyHal> legacy_hal)
-    : chip_id_(chip_id), legacy_hal_(legacy_hal), is_valid_(true) {}
+WifiChip::WifiChip(
+    ChipId chip_id,
+    const std::weak_ptr<legacy_hal::WifiLegacyHal> legacy_hal,
+    const std::weak_ptr<mode_controller::WifiModeController> mode_controller)
+    : chip_id_(chip_id),
+      legacy_hal_(legacy_hal),
+      mode_controller_(mode_controller),
+      is_valid_(true),
+      current_mode_id_(kInvalidModeId) {}
 
 void WifiChip::invalidate() {
   invalidateAndRemoveAllIfaces();
@@ -301,19 +313,84 @@
 
 std::pair<WifiStatus, std::vector<IWifiChip::ChipMode>>
 WifiChip::getAvailableModesInternal() {
-  // TODO add implementation
-  return {createWifiStatus(WifiStatusCode::SUCCESS), {}};
+  // The chip combination supported for current devices is fixed for now with
+  // 2 separate modes of operation:
+  // Mode 1 (STA mode): Will support 1 STA and 1 P2P or NAN iface operations
+  // concurrently.
+  // Mode 2 (AP mode): Will support 1 AP iface operations.
+  // TODO (b/32997844): Read this from some device specific flags in the
+  // makefile.
+  // STA mode iface combinations.
+  const IWifiChip::ChipIfaceCombinationLimit
+      sta_chip_iface_combination_limit_1 = {{IfaceType::STA}, 1};
+  const IWifiChip::ChipIfaceCombinationLimit
+      sta_chip_iface_combination_limit_2 = {{IfaceType::P2P, IfaceType::NAN},
+                                            1};
+  const IWifiChip::ChipIfaceCombination sta_chip_iface_combination = {
+      {sta_chip_iface_combination_limit_1, sta_chip_iface_combination_limit_2}};
+  const IWifiChip::ChipMode sta_chip_mode = {kStaChipModeId,
+                                             {sta_chip_iface_combination}};
+  // AP mode iface combinations.
+  const IWifiChip::ChipIfaceCombinationLimit ap_chip_iface_combination_limit = {
+      {IfaceType::AP}, 1};
+  const IWifiChip::ChipIfaceCombination ap_chip_iface_combination = {
+      {ap_chip_iface_combination_limit}};
+  const IWifiChip::ChipMode ap_chip_mode = {kApChipModeId,
+                                            {ap_chip_iface_combination}};
+  return {createWifiStatus(WifiStatusCode::SUCCESS),
+          {sta_chip_mode, ap_chip_mode}};
 }
 
-WifiStatus WifiChip::configureChipInternal(uint32_t /* mode_id */) {
-  invalidateAndRemoveAllIfaces();
-  // TODO add implementation
+WifiStatus WifiChip::configureChipInternal(uint32_t mode_id) {
+  if (mode_id != kStaChipModeId && mode_id != kApChipModeId) {
+    return createWifiStatus(WifiStatusCode::ERROR_INVALID_ARGS);
+  }
+  if (mode_id == current_mode_id_) {
+    LOG(DEBUG) << "Already in the specified mode " << mode_id;
+    return createWifiStatus(WifiStatusCode::SUCCESS);
+  }
+  // If the chip is already configured in a different mode, stop
+  // the legacy HAL and then start it after firmware mode change.
+  if (current_mode_id_ != kInvalidModeId) {
+    invalidateAndRemoveAllIfaces();
+    legacy_hal::wifi_error legacy_status = legacy_hal_.lock()->stop([]() {});
+    if (legacy_status != legacy_hal::WIFI_SUCCESS) {
+      LOG(ERROR) << "Failed to stop legacy HAL: "
+                 << legacyErrorToString(legacy_status);
+      // TODO(b/33038823): Need to invoke onChipReconfigureFailure()
+      return createWifiStatusFromLegacyError(legacy_status);
+    }
+  }
+  bool success;
+  if (mode_id == kStaChipModeId) {
+    success = mode_controller_.lock()->changeFirmwareMode(IfaceType::STA);
+  } else {
+    success = mode_controller_.lock()->changeFirmwareMode(IfaceType::AP);
+  }
+  if (!success) {
+    // TODO(b/33038823): Need to invoke onChipReconfigureFailure()
+    return createWifiStatus(WifiStatusCode::ERROR_UNKNOWN);
+  }
+  legacy_hal::wifi_error legacy_status = legacy_hal_.lock()->start();
+  if (legacy_status != legacy_hal::WIFI_SUCCESS) {
+    LOG(ERROR) << "Failed to start legacy HAL: "
+               << legacyErrorToString(legacy_status);
+    // TODO(b/33038823): Need to invoke onChipReconfigureFailure()
+    return createWifiStatusFromLegacyError(legacy_status);
+  }
+  for (const auto& callback : event_callbacks_) {
+    callback->onChipReconfigured(mode_id);
+  }
+  current_mode_id_ = mode_id;
   return createWifiStatus(WifiStatusCode::SUCCESS);
 }
 
 std::pair<WifiStatus, uint32_t> WifiChip::getModeInternal() {
-  // TODO add implementation
-  return {createWifiStatus(WifiStatusCode::SUCCESS), 0};
+  if (current_mode_id_ == kInvalidModeId) {
+    return {createWifiStatus(WifiStatusCode::ERROR_NOT_AVAILABLE),
+            current_mode_id_};
+  }
+  return {createWifiStatus(WifiStatusCode::SUCCESS), current_mode_id_};
 }
 
 std::pair<WifiStatus, IWifiChip::ChipDebugInfo>
diff --git a/wifi/1.0/default/wifi_chip.h b/wifi/1.0/default/wifi_chip.h
index c1a7173..764445f 100644
--- a/wifi/1.0/default/wifi_chip.h
+++ b/wifi/1.0/default/wifi_chip.h
@@ -24,6 +24,7 @@
 
 #include "wifi_ap_iface.h"
 #include "wifi_legacy_hal.h"
+#include "wifi_mode_controller.h"
 #include "wifi_nan_iface.h"
 #include "wifi_p2p_iface.h"
 #include "wifi_rtt_controller.h"
@@ -42,8 +43,10 @@
  */
 class WifiChip : public IWifiChip {
  public:
-  WifiChip(ChipId chip_id,
-           const std::weak_ptr<legacy_hal::WifiLegacyHal> legacy_hal);
+  WifiChip(
+      ChipId chip_id,
+      const std::weak_ptr<legacy_hal::WifiLegacyHal> legacy_hal,
+      const std::weak_ptr<mode_controller::WifiModeController> mode_controller);
   // HIDL does not provide a built-in mechanism to let the server invalidate
   // a HIDL interface object after creation. If any client process holds onto
   // a reference to the object in their context, any method calls on that
@@ -156,6 +159,7 @@
 
   ChipId chip_id_;
   std::weak_ptr<legacy_hal::WifiLegacyHal> legacy_hal_;
+  std::weak_ptr<mode_controller::WifiModeController> mode_controller_;
   std::vector<sp<IWifiChipEventCallback>> event_callbacks_;
   sp<WifiApIface> ap_iface_;
   sp<WifiNanIface> nan_iface_;
@@ -163,6 +167,7 @@
   sp<WifiStaIface> sta_iface_;
   std::vector<sp<WifiRttController>> rtt_controllers_;
   bool is_valid_;
+  uint32_t current_mode_id_;
 
   DISALLOW_COPY_AND_ASSIGN(WifiChip);
 };