WiimoteEmu: Add logic for enabling/disabling M+ and force it disabled for now.

Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp

@@ -16,6 +16,7 @@
 #include "Core/HW/WiimoteReal/WiimoteReal.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Attachments.h"
 #include "InputCommon/ControllerEmu/ControlGroup/ModifySettingsButton.h"
+#include "InputCommon/ControllerEmu/Setting/BooleanSetting.h"
 
 namespace WiimoteEmu
 {
@@ -145,12 +146,53 @@ void Wiimote::SendAck(OutputReportID rpt_id, ErrorCode error_code)
 
 void Wiimote::HandleExtensionSwap()
 {
-  const ExtensionNumber desired_extension =
+  ExtensionNumber desired_extension_number =
       static_cast<ExtensionNumber>(m_attachments->GetSelectedAttachment());
 
-  if (GetActiveExtensionNumber() != desired_extension)
+  // const bool desired_motion_plus = m_motion_plus_setting->GetValue();
+  const bool desired_motion_plus = false;
+
+  // FYI: AttachExtension also connects devices to the i2c bus
+
+  if (m_is_motion_plus_attached && !desired_motion_plus)
   {
-    if (GetActiveExtensionNumber())
+    // M+ is attached and it's not wanted, so remove it.
+    m_extension_port.AttachExtension(GetNoneExtension());
+    m_is_motion_plus_attached = false;
+
+    // Also remove extension (if any) from the M+'s ext port.
+    m_active_extension = ExtensionNumber::NONE;
+    m_motion_plus.GetExtPort().AttachExtension(GetNoneExtension());
+
+    // Don't do anything else this update cycle.
+    return;
+  }
+
+  if (desired_motion_plus && !m_is_motion_plus_attached)
+  {
+    // M+ is wanted and it's not attached
+
+    if (GetActiveExtensionNumber() != ExtensionNumber::NONE)
+    {
+      // But an extension is attached. Remove it first.
+      // (handled below)
+      desired_extension_number = ExtensionNumber::NONE;
+    }
+    else
+    {
+      // No extension attached so attach M+.
+      m_is_motion_plus_attached = true;
+      m_extension_port.AttachExtension(&m_motion_plus);
+      m_motion_plus.Reset();
+
+      // Also attach extension below if desired:
+    }
+  }
+
+  if (GetActiveExtensionNumber() != desired_extension_number)
+  {
+    // A different extension is wanted (either by user or by the M+ logic above)
+    if (GetActiveExtensionNumber() != ExtensionNumber::NONE)
     {
       // First we must detach the current extension.
       // The next call will change to the new extension if needed.
@@ -158,11 +200,20 @@ void Wiimote::HandleExtensionSwap()
     }
     else
     {
-      m_active_extension = desired_extension;
+      m_active_extension = desired_extension_number;
+    }
+
+    if (m_is_motion_plus_attached)
+    {
+      // M+ is attached so attach to it.
+      m_motion_plus.GetExtPort().AttachExtension(GetActiveExtension());
+    }
+    else
+    {
+      // M+ is not attached so attach directly.
+      m_extension_port.AttachExtension(GetActiveExtension());
     }
 
-    // TODO: Attach directly when not using M+.
-    m_motion_plus.AttachExtension(GetActiveExtension());
     GetActiveExtension()->Reset();
   }
 }
@@ -503,14 +554,18 @@ void Wiimote::DoState(PointerWrap& p)
 
   // Sub-devices:
   m_speaker_logic.DoState(p);
-  m_motion_plus.DoState(p);
   m_camera_logic.DoState(p);
 
+  p.Do(m_is_motion_plus_attached);
   p.Do(m_active_extension);
-  GetActiveExtension()->DoState(p);
 
-  // TODO: Handle motion plus being disabled.
+  // Attach M+/Extensions.
-  m_motion_plus.AttachExtension(GetActiveExtension());
+  m_extension_port.AttachExtension(m_is_motion_plus_attached ? &m_motion_plus : GetNoneExtension());
+  (m_is_motion_plus_attached ? m_motion_plus.GetExtPort() : m_extension_port)
+      .AttachExtension(GetActiveExtension());
+
+  m_motion_plus.DoState(p);
+  GetActiveExtension()->DoState(p);
 
   // Dynamics
   // TODO: clean this up:

Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp

@@ -50,11 +50,6 @@ void MotionPlus::DoState(PointerWrap& p)
   p.Do(reg_data);
 }
 
-void MotionPlus::AttachExtension(Extension* ext)
-{
-  extension_port.AttachExtension(ext);
-}
-
 bool MotionPlus::IsActive() const
 {
   return ACTIVE_DEVICE_ADDR << 1 == reg_data.ext_identifier[2];
@@ -65,6 +60,11 @@ MotionPlus::PassthroughMode MotionPlus::GetPassthroughMode() const
   return static_cast<PassthroughMode>(reg_data.ext_identifier[4]);
 }
 
+ExtensionPort& MotionPlus::GetExtPort()
+{
+  return m_extension_port;
+}
+
 int MotionPlus::BusRead(u8 slave_addr, u8 addr, int count, u8* data_out)
 {
   if (IsActive())
@@ -175,7 +175,7 @@ bool MotionPlus::ReadDeviceDetectPin() const
   else
   {
     // When inactive the device detect pin reads from the ext port:
-    return extension_port.IsDeviceConnected();
+    return m_extension_port.IsDeviceConnected();
   }
 }
 
@@ -369,7 +369,7 @@ void MotionPlus::Update()
     mplus_data.pitch2 = pitch_value >> 8;
   }
 
-  mplus_data.extension_connected = extension_port.IsDeviceConnected();
+  mplus_data.extension_connected = m_extension_port.IsDeviceConnected();
   mplus_data.zero = 0;
 }
 

Source/Core/Core/HW/WiimoteEmu/MotionPlus.h

@@ -21,7 +21,7 @@ public:
   void Reset() override;
   void DoState(PointerWrap& p) override;
 
-  void AttachExtension(Extension* ext);
+  ExtensionPort& GetExtPort();
 
 private:
 #pragma pack(push, 1)
@@ -126,6 +126,6 @@ private:
 
   // The port on the end of the motion plus:
   I2CBus i2c_bus;
-  ExtensionPort extension_port{&i2c_bus};
+  ExtensionPort m_extension_port{&i2c_bus};
 };
 }  // namespace WiimoteEmu

Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp

@@ -112,13 +112,13 @@ void Wiimote::Reset()
   m_i2c_bus.AddSlave(&m_speaker_logic);
   m_i2c_bus.AddSlave(&m_camera_logic);
 
-  // FYI: AttachExtension also connects devices to the i2c bus
+  // Reset extension connections:
-  // TODO: Only attach M+ when enabled in settings.
+  m_is_motion_plus_attached = false;
-  m_extension_port.AttachExtension(&m_motion_plus);
-
   m_active_extension = ExtensionNumber::NONE;
-  m_motion_plus.AttachExtension(GetActiveExtension());
+  m_extension_port.AttachExtension(GetNoneExtension());
-  // Switch to the desired extension (if any).
+  m_motion_plus.GetExtPort().AttachExtension(GetNoneExtension());
+
+  // Switch to desired M+ status and extension (if any).
   HandleExtensionSwap();
 
   // Reset sub-devices:
@@ -215,6 +215,13 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index)
 
   // options
   groups.emplace_back(m_options = new ControllerEmu::ControlGroup(_trans("Options")));
+
+  // m_options->boolean_settings.emplace_back(
+  //    m_motion_plus_setting =
+  //        new ControllerEmu::BooleanSetting("Attach MotionPlus", _trans("Attach MotionPlus"),
+  //        true,
+  //                                          ControllerEmu::SettingType::NORMAL, false));
+
   m_options->boolean_settings.emplace_back(
       new ControllerEmu::BooleanSetting("Forward Wiimote", _trans("Forward Wii Remote"), true,
                                         ControllerEmu::SettingType::NORMAL, true));
@@ -673,6 +680,11 @@ void Wiimote::LoadDefaults(const ControllerInterface& ciface)
   m_attachments->GetAttachmentList()[DEFAULT_EXT]->LoadDefaults(ciface);
 }
 
+Extension* Wiimote::GetNoneExtension() const
+{
+  return static_cast<Extension*>(m_attachments->GetAttachmentList()[ExtensionNumber::NONE].get());
+}
+
 Extension* Wiimote::GetActiveExtension() const
 {
   return static_cast<Extension*>(m_attachments->GetAttachmentList()[m_active_extension].get());

Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h

@@ -181,6 +181,7 @@ private:
   bool IsUpright() const;
 
   Extension* GetActiveExtension() const;
+  Extension* GetNoneExtension() const;
 
   bool NetPlay_GetWiimoteData(int wiimote, u8* data, u8 size, u8 reporting_mode);
 
@@ -247,6 +248,7 @@ private:
   ControllerEmu::BooleanSetting* m_sideways_setting;
   ControllerEmu::BooleanSetting* m_upright_setting;
   ControllerEmu::NumericSetting* m_battery_setting;
+  // ControllerEmu::BooleanSetting* m_motion_plus_setting;
   ControllerEmu::ModifySettingsButton* m_hotkeys;
 
   SpeakerLogic m_speaker_logic;
@@ -273,6 +275,8 @@ private:
 
   ExtensionNumber m_active_extension;
 
+  bool m_is_motion_plus_attached;
+
   ReadRequest m_read_request;
   UsableEEPROMData m_eeprom;