Copied some emulated motion plus stuff over from the old wiimote plugin. Not at all functional currently.

git-svn-id: https://dolphin-emu.googlecode.com/svn/trunk@6660 8ced0084-cf51-0410-be5f-012b33b47a6e

Source/Core/Core/Src/HW/WiimoteEmu/Attachment/Nunchuk.h

@@ -30,7 +30,7 @@ private:
 	Buttons*		m_buttons;
 	AnalogStick*	m_stick;
 
-	unsigned int	m_shake_step[3];
+	u8	m_shake_step[3];
 	
 	UDPWrapper* const m_udpWrap;
 };

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

@@ -215,11 +215,11 @@ void Wiimote::HandleExtensionSwap()
 			// set the wanted extension
 			m_extension->active_extension = m_extension->switch_extension;
 
-		// update status struct
+		// set register, I hate this
-		m_status.extension = m_extension->active_extension ? 1 : 0;
+		const std::vector<u8> &reg = ((WiimoteEmu::Attachment*)m_extension->attachments[m_extension->active_extension])->reg;
+		memset(&m_reg_ext, 0, WIIMOTE_REG_EXT_SIZE);
+		memcpy(&m_reg_ext, &reg[0], reg.size());
 
-		// set register, I hate this line
-		m_register[0xa40000] = ((WiimoteEmu::Attachment*)m_extension->attachments[m_extension->active_extension])->reg;
 	}
 }
 
@@ -232,6 +232,9 @@ void Wiimote::RequestStatus(const wm_request_status* const rs)
 {
 	HandleExtensionSwap();
 
+	// update status struct
+	m_status.extension = (m_extension->active_extension || m_motion_plus_active) ? 1 : 0;
+
 	// set up report
 	u8 data[8];
 	data[0] = 0xA1;
@@ -267,7 +270,7 @@ void Wiimote::WriteData(const wm_write_data* const wd)
 	u32 address = swap24(wd->address);
 
 	// ignore the 0x010000 bit
-	address &= 0xFEFFFF;
+	address &= ~0x010000;
 
 	if (wd->size > 16)
 	{
@@ -301,6 +304,7 @@ void Wiimote::WriteData(const wm_write_data* const wd)
 			}
 		}
 		break;
+		
 	case WM_SPACE_REGS1 :
 	case WM_SPACE_REGS2 :
 		{
@@ -310,36 +314,72 @@ void Wiimote::WriteData(const wm_write_data* const wd)
 			if (0xA4 == (address >> 16))
 				address &= 0xFF00FF;
 
-			// write to the register
+			const u8 region_offset = (u8)address;
-			m_register.Write(address, wd->data, wd->size);
+			void *region_ptr = NULL;
+			int region_size = 0;
 
 			switch (address >> 16)
 			{
 			// speaker
 			case 0xa2 :
-				//PanicAlert("Write to speaker!!");
+				region_ptr = &m_reg_speaker;
+				region_size = WIIMOTE_REG_SPEAKER_SIZE;
 				break;
+
 			// extension register
 			case 0xa4 :
+				region_ptr = m_motion_plus_active ? (void*)&m_reg_motion_plus : (void*)&m_reg_ext;
+				region_size = WIIMOTE_REG_EXT_SIZE;
+				break;
+
+			// motion plus
+			case 0xa6 :
+				if (false == m_motion_plus_active)
 				{
-					// Run the key generation on all writes in the key area, it doesn't matter 
+					region_ptr = &m_reg_motion_plus;
-					// that we send it parts of a key, only the last full key will have an effect
+					region_size = WIIMOTE_REG_EXT_SIZE;
-					// I might have f'ed this up
-					if ( address >= 0xa40040 && address <= 0xa4004c )
-						wiimote_gen_key(&m_ext_key, m_reg_ext->encryption_key);
-					//else if ( address >= 0xa40020 && address < 0xa40040 )
-					//	PanicAlert("Writing to extension calibration data! Extension may misbehave");
 				}
 				break;
+
 			// ir
 			case 0xB0 :
-				if (5 == m_reg_ir->mode)
+				region_ptr = &m_reg_ir;
-					PanicAlert("IR Full Mode is Unsupported!");
+				region_size = WIIMOTE_REG_IR_SIZE;
+
+				//if (5 == m_reg_ir->mode)
+				//	PanicAlert("IR Full Mode is Unsupported!");
 				break;
 			}
 
+			if (region_ptr && (region_offset + wd->size <= region_size))
+			{
+				memcpy((u8*)region_ptr + region_offset, wd->data, wd->size);
+			}
+			else
+				return;	// TODO: generate a writedata error reply
+
+			if (&m_reg_ext == region_ptr)
+			{
+				// Run the key generation on all writes in the key area, it doesn't matter 
+				// that we send it parts of a key, only the last full key will have an effect
+				if (address >= 0xa40040 && address <= 0xa4004c)
+					wiimote_gen_key(&m_ext_key, m_reg_ext.encryption_key);
+			}
+			else if (&m_reg_motion_plus == region_ptr)
+			{
+				// activate/deactivate motion plus
+				if (0x55 == m_reg_motion_plus.activated)
+				{
+					// maybe hacky
+					m_reg_motion_plus.activated = 0;
+					m_motion_plus_active ^= 1;
+
+					RequestStatus();
+				}
+			}
 		}
 		break;
+
 	default:
 		PanicAlert("WriteData: unimplemented parameters!");
 		break;
@@ -366,7 +406,7 @@ void Wiimote::ReadData(const wm_read_data* const rd)
 	}
 
 	ReadRequest rr;
-	u8* block = new u8[size];
+	u8 *const block = new u8[size];
 
 	switch (rd->space)
 	{
@@ -391,15 +431,16 @@ void Wiimote::ReadData(const wm_read_data* const rd)
 			{
 				// reading the whole mii block :/
 				std::ifstream file;
-				file.open( (std::string(File::GetUserPath(D_WIIUSER_IDX)) + "mii.bin").c_str(), std::ios::binary | std::ios::in);
+				file.open((std::string(File::GetUserPath(D_WIIUSER_IDX)) + "mii.bin").c_str(), std::ios::binary | std::ios::in);
 				file.read((char*)m_eeprom + 0x0FCA, 0x02f0);
 				file.close();
 			}
 
 			// read mem to be sent to wii
-			memcpy( block, m_eeprom + address, size);
+			memcpy(block, m_eeprom + address, size);
 		}
 		break;
+
 	case WM_SPACE_REGS1 :
 	case WM_SPACE_REGS2 :
 		{
@@ -409,44 +450,58 @@ void Wiimote::ReadData(const wm_read_data* const rd)
 			if (0xA4 == (address >> 16))
 				address &= 0xFF00FF;
 
-			// read block to send to wii
+			const u8 region_offset = (u8)address;
-			m_register.Read( address, block, size );
+			void *region_ptr = NULL;
+			int region_size = 0;
 
 			switch (address >> 16)
 			{
 			// speaker
-			case 0xa2 :
+			case 0xa2:
-				//PanicAlert("read from speaker!!");
+				region_ptr = &m_reg_speaker;
+				region_size = WIIMOTE_REG_SPEAKER_SIZE;
 				break;
+
 			// extension
-			case 0xa4 :
+			case 0xa4:
+				region_ptr = m_motion_plus_active ? (void*)&m_reg_motion_plus : (void*)&m_reg_ext;
+				region_size = WIIMOTE_REG_EXT_SIZE;
+				break;
+
+			// motion plus
+			case 0xa6:
+				// reading from 0xa6 returns error when mplus is activated
+				if (false == m_motion_plus_active)
+				{
+					region_ptr = &m_reg_motion_plus;
+					region_size = WIIMOTE_REG_EXT_SIZE;
+				}
+				break;
+
+			// ir
+			case 0xb0:
+				region_ptr = &m_reg_ir;
+				region_size = WIIMOTE_REG_IR_SIZE;
+				break;
+			}
+
+			if (region_ptr && (region_offset + size <= region_size))
+			{
+				memcpy(block, (u8*)region_ptr + region_offset, size);
+			}
+			else
+				size = 0;	// generate read error
+
+			if (&m_reg_ext == region_ptr)
 			{
 				// Encrypt data read from extension register
 				// Check if encrypted reads is on
-					if (0xaa == m_reg_ext->encryption)
+				if (0xaa == m_reg_ext.encryption)
 					wiimote_encrypt(&m_ext_key, block, address & 0xffff, (u8)size);
+			}
+		}
+		break;
 
-					//if ( address >= 0xa40008 && address < 0xa40020 )
-					//	PanicAlert("Reading extension data from register");
-				}
-				break;
-			// motion plus
-			case 0xa6 :
-				{
-					// emulated motion plus is not yet supported
-					// return read error
-					size = 0;
-
-					// motion plus crap copied from old wiimote plugin
-					//block[0xFC] = 0xA6;
-					//block[0xFD] = 0x20;
-					//block[0xFE] = 0x00;
-					//block[0xFF] = 0x05;
-				}
-				break;
-			}
-		} 
-		break;
 	default :
 		PanicAlert("WmReadData: unimplemented parameters (size: %i, addr: 0x%x)!", size, rd->space);
 		break;

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

@@ -72,7 +72,7 @@ const ReportFeatures reporting_mode_features[] =
 
 void EmulateShake( AccelData* const accel
 	  , ControllerEmu::Buttons* const buttons_group
-	  , unsigned int* const shake_step )
+	  , u8* const shake_step )
 {
 	static const double shake_data[] = { -2.5f, -5.0f, -2.5f, 0.0f, 2.5f, 5.0f, 2.5f, 0.0f };
 	static const unsigned int btns[] = { 0x01, 0x02, 0x04 };
@@ -183,6 +183,8 @@ void Wiimote::Reset()
 
 	m_rumble_on = false;
 	m_speaker_mute = false;
+	m_motion_plus_present = false;
+	m_motion_plus_active = false;
 
 	// will make the first Update() call send a status request
 	// the first call to RequestStatus() will then set up the status struct extension bit
@@ -196,19 +198,12 @@ void Wiimote::Reset()
 	memcpy(m_eeprom + 0x16D0, eeprom_data_16D0, sizeof(eeprom_data_16D0));
 
 	// set up the register
-	m_register.clear();
+	memset(&m_reg_speaker, 0, sizeof(m_reg_speaker));
-	m_register[0xa20000].resize(WIIMOTE_REG_SPEAKER_SIZE,0);
+	memset(&m_reg_ir, 0, sizeof(m_reg_ir));
-	m_register[0xa40000].resize(WIIMOTE_REG_EXT_SIZE,0);
+	memset(&m_reg_ext, 0, sizeof(m_reg_ext));
-	m_register[0xa60000].resize(WIIMOTE_REG_EXT_SIZE,0);
+	memset(&m_reg_motion_plus, 0, sizeof(m_reg_motion_plus));
-	m_register[0xB00000].resize(WIIMOTE_REG_IR_SIZE,0);
 
-	m_reg_speaker		= (SpeakerReg*)&m_register[0xa20000][0];
+	memcpy(&m_reg_motion_plus.ext_identifier, motion_plus_id, sizeof(motion_plus_id));
-	m_reg_ext			= (ExtensionReg*)&m_register[0xa40000][0];
-	m_reg_motion_plus	= &m_register[0xa60000][0];
-	m_reg_ir			= (IrReg*)&m_register[0xB00000][0];
-
-	// testing
-	//memcpy(m_reg_motion_plus + 0xfa, motion_plus_id, sizeof(motion_plus_id));
 
 	// status
 	memset(&m_status, 0, sizeof(m_status));
@@ -269,6 +264,8 @@ Wiimote::Wiimote( const unsigned int index )
 	m_extension->attachments.push_back(new WiimoteEmu::Drums());
 	m_extension->attachments.push_back(new WiimoteEmu::Turntable());
 
+	m_extension->settings.push_back(new ControlGroup::Setting("Motion Plus", 0, 0, 1));
+
 	// rumble
 	groups.push_back(m_rumble = new ControlGroup("Rumble"));
 	m_rumble->controls.push_back(new ControlGroup::Output("Motor"));
@@ -327,6 +324,9 @@ bool Wiimote::Step()
 	const bool has_focus = HAS_FOCUS;
 	const bool is_sideways = m_options->settings[1]->value != 0;
 
+	// TODO: change this a bit
+	m_motion_plus_present = m_extension->settings[0]->value != 0;
+
 	// no rumble if no focus
 	if (false == has_focus)
 		m_rumble_on = false;
@@ -517,9 +517,9 @@ void Wiimote::GetIRData(u8* const data, bool use_accel)
 	//		x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[38]);
 	}
 	// Fill report with valid data when full handshake was done
-	if (m_reg_ir->data[0x30])
+	if (m_reg_ir.data[0x30])
 	// ir mode
-	switch (m_reg_ir->mode)
+	switch (m_reg_ir.mode)
 	{
 	// basic
 	case 1 :
@@ -579,9 +579,46 @@ void Wiimote::GetExtData(u8* const data)
 
 	// i dont think anything accesses the extension data like this, but ill support it. Indeed, commercial games don't do this.
 	// i think it should be unencrpyted in the register, encrypted when read.
-	memcpy(m_reg_ext->controller_data, data, sizeof(wm_extension));
+	memcpy(m_reg_ext.controller_data, data, sizeof(wm_extension));
 
-	if (0xAA == m_reg_ext->encryption)
+	// motionplus pass-through modes
+	if (m_motion_plus_active)
+	{
+		switch (m_reg_motion_plus.ext_identifier[0x4])
+		{
+		// nunchuck pass-through mode
+		// Bit 7 of byte 5 is moved to bit 6 of byte 5, overwriting it
+		// Bit 0 of byte 4 is moved to bit 7 of byte 5
+		// Bit 3 of byte 5 is moved to bit 4 of byte 5, overwriting it
+		// Bit 1 of byte 5 is moved to bit 3 of byte 5
+		// Bit 0 of byte 5 is moved to bit 2 of byte 5, overwriting it 
+		case 0x5:
+			//data[5] & (1 << 7)
+			//data[4] & (1 << 0)
+			//data[5] & (1 << 3)
+			//data[5] & (1 << 1)
+			//data[5] & (1 << 0)
+			break;
+
+		// classic controller/musical instrument pass-through mode
+		// Bit 0 of Byte 4 is overwritten
+		// Bits 0 and 1 of Byte 5 are moved to bit 0 of Bytes 0 and 1, overwriting
+		case 0x7:
+			//data[4] & (1 << 0)
+			//data[5] & (1 << 0)
+			//data[5] & (1 << 1)
+			break;
+
+		// unknown pass-through mode
+		default:
+			break;
+		}
+
+		((wm_motionplus_data*)data)->is_mp_data = 0;
+		((wm_motionplus_data*)data)->extension_connected = m_extension->active_extension;
+	}
+
+	if (0xAA == m_reg_ext.encryption)
 		wiimote_encrypt(&m_ext_key, data, 0x00, sizeof(wm_extension));
 }
 
@@ -896,108 +933,4 @@ void Wiimote::LoadDefaults(const ControllerInterface& ciface)
 
 }
 
-// TODO: i need to test this
-void Wiimote::Register::Read( size_t address, void* dst, size_t length )
-{
-	const_iterator i = begin();
-	const const_iterator e = end();
-	while (length)
-	{
-		const std::vector<u8>* block = NULL;
-		size_t addr_start = 0;
-		size_t addr_end = address+length;
-
-		// find block and start of next block
-		for ( ; i!=e; ++i )
-			// if address is inside or after this block
-			if ( address >= i->first )
-			{
-				block = &i->second;
-				addr_start = i->first;
-			}
-			// if address is before this block
-			else
-			{
-				// how far til the start of the next block
-				addr_end = std::min( i->first, addr_end );
-				break;
-			}
-
-		// read bytes from a mapped block
-		if (block)
-		{
-			// offset of wanted data in the vector
-			const size_t offset = std::min( address - addr_start, block->size() );
-			// how much data we can read depending on the vector size and how much we want
-			const size_t amt = std::min( block->size()-offset, length );
-
-			memcpy( dst, &block->operator[](offset), amt );
-			
-			address += amt;
-			dst = ((u8*)dst) + amt;
-			length -= amt;
-		}
-
-		// read zeros for unmapped regions
-		const size_t amt = addr_end - address;
-
-		memset( dst, 0, amt );
-
-		address += amt;
-		dst = ((u8*)dst) + amt;
-		length -= amt;
-	}
-}
-
-// TODO: i need to test this
-void Wiimote::Register::Write( size_t address, const void* src, size_t length )
-{
-	iterator i = begin();
-	const const_iterator e = end();
-	while (length)
-	{
-		std::vector<u8>* block = NULL;
-		size_t addr_start = 0;
-		size_t addr_end = address+length;
-
-		// find block and start of next block
-		for ( ; i!=e; ++i )
-			// if address is inside or after this block
-			if ( address >= i->first )
-			{
-				block = &i->second;
-				addr_start = i->first;
-			}
-			// if address is before this block
-			else
-			{
-				// how far til the start of the next block
-				addr_end = std::min( i->first, addr_end );
-				break;
-			}
-
-		// write bytes to a mapped block
-		if (block)
-		{
-			// offset of wanted data in the vector
-			const size_t offset = std::min( address - addr_start, block->size() );
-			// how much data we can read depending on the vector size and how much we want
-			const size_t amt = std::min( block->size()-offset, length );
-
-			memcpy( &block->operator[](offset), src, amt );
-			
-			address += amt;
-			src = ((u8*)src) + amt;
-			length -= amt;
-		}
-
-		// do nothing for unmapped regions
-		const size_t amt = addr_end - address;
-
-		address += amt;
-		src = ((u8*)src) + amt;
-		length -= amt;
-	}
-}
-
 }

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

@@ -46,7 +46,7 @@ extern const ReportFeatures reporting_mode_features[];
 
 void EmulateShake(AccelData* const accel_data
 	  , ControllerEmu::Buttons* const buttons_group
-	  , unsigned int* const shake_step);
+	  , u8* const shake_step);
 
 void EmulateTilt(AccelData* const accel
 	 , ControllerEmu::Tilt* const tilt_group
@@ -129,44 +129,37 @@ private:
 #endif
 
 	// control groups
-	Buttons*				m_buttons;
+	Buttons		*m_buttons, *m_dpad, *m_shake;
-	Buttons*				m_dpad;
-	Buttons*				m_shake;
 	Cursor*			m_ir;
 	Tilt*			m_tilt;
 	Force*			m_swing;
 	ControlGroup*	m_rumble;
 	Extension*		m_extension;
 	ControlGroup*	m_options;
+	
 	// WiiMote accel data
 	AccelData		m_accel;
 
 	// wiimote index, 0-3
-	const unsigned int		m_index;
+	const u8	m_index;
 
-	double					ir_sin,ir_cos; //for the low pass filter
+	double		ir_sin, ir_cos; //for the low pass filter
 	
 	UDPWrapper* m_udp;
 
 	bool	m_rumble_on;
 	bool	m_speaker_mute;
+	bool	m_motion_plus_present;
+	bool	m_motion_plus_active;
 
 	bool	m_reporting_auto;
 	u8		m_reporting_mode;
 	u16		m_reporting_channel;
 
-	unsigned int			m_shake_step[3];
+	u8		m_shake_step[3];
 
 	wm_status_report		m_status;
 
-	class Register : public std::map< size_t, std::vector<u8> >
-	{
-	public:
-		void Write( size_t address, const void* src, size_t length );
-		void Read( size_t address, void* dst, size_t length );
-
-	} m_register;
-
 	// read data request queue
 	// maybe it isn't actualy a queue
 	// maybe read requests cancel any current requests
@@ -187,16 +180,30 @@ private:
 	}	m_channel_status;
 #endif
 
+	wiimote_key		m_ext_key;
+
 	u8		m_eeprom[WIIMOTE_EEPROM_SIZE];
 
-	u8*		m_reg_motion_plus;
+	struct MotionPlusReg
+	{
+		u8 unknown[0xF0];
+
+		// address 0xF0
+		u8	activated;
+
+		u8 unknown2[9];
+
+		// address 0xFA
+		u8	ext_identifier[6];
+
+	}	m_reg_motion_plus;
 
 	struct IrReg
 	{
 		u8	data[0x33];
 		u8	mode;
 
-	}	*m_reg_ir;
+	}	m_reg_ir;
 
 	struct ExtensionReg
 	{
@@ -221,7 +228,7 @@ private:
 		// address 0xFA
 		u8	constant_id[6];
 
-	}	*m_reg_ext;
+	}	m_reg_ext;
 
 	struct SpeakerReg
 	{
@@ -231,9 +238,7 @@ private:
 		u8		volume;
 		u8		unk[4];
 
-	}	*m_reg_speaker;
+	}	m_reg_speaker;
-
-	wiimote_key		m_ext_key;
 };
 
 }

Source/Core/Core/Src/HW/WiimoteEmu/WiimoteHid.h

@@ -159,6 +159,27 @@ struct wm_turntable_extension
 	};
 };
 
+struct wm_motionplus_data
+{
+	u8 yaw1;
+	
+	u8 roll1;
+	
+	u8 pitch1;
+
+	u8 yaw2 : 6;
+	u8 yaw_slow : 1;
+	u8 pitch_slow : 1;
+
+	u8 roll2 : 6;
+	u8 roll_slow : 1;
+	u8 extension_connected : 1;
+
+	u8 pitch2 : 6;
+	u8 is_mp_data : 1;
+	u8 zero : 1;
+};
+
 struct wm_report
 {
 	u8 wm;