public static void ReadI2CWorker(object data)
{
WiiReaderV1 reader = ((WiiReaderV1)data);
while (!reader.inShutdown)
{
I2CPacket packet = reader.packetPool[reader.currentPacketInPool];
reader.currentPacketInPool += 1;
reader.currentPacketInPool %= 100;
uint status = 0;
ulong timeSop = 0;
ulong timeDuration = 0;
uint timeDataOffset = 0;
packet.Length = BeagleApi.bg_i2c_read(
((WiiReaderV1)data).hBeagle, ref status, ref timeSop,
ref timeDuration, ref timeDataOffset, 1024, packet.Data);
if (packet.Length >= 1)
{
reader.packetsToBeProcessed.Enqueue(packet);
}
}
}
public static void ProcessPacketWorker(object data)
{
WiiReaderV1 reader = ((WiiReaderV1)data);
while (!reader.inShutdown)
{
if (reader.packetsToBeProcessed.Count > 0)
{
reader.packetsToBeProcessed.TryDequeue(out I2CPacket packet);
// Check the address
if (((packet.Data[0] & 0xff) >> 1) != 0x52)
{
continue;
}
// read or write
bool isWrite = (packet.Data[0] & 0x01) == 0;
if (isWrite)
{
byte data1 = (byte)(packet.Data[1] & 0xFF);
if (packet.Length == 2 && data1 == 0)
{
reader.isControllerPoll = true;
}
else if (packet.Length == 3 && (byte)(data1 & 0xFF) == 0xF0 && (byte)(packet.Data[2] & 0xFF) == 0x55)
{
reader.isEncrypted = false;
}
else if (packet.Length == 2 && (byte)(data1 & 0xFF) == 0xFA)
{
reader.isControllerID = true;
}
else if ((data1 == 0x40 && packet.Length == 8) || (data1 == 0x46 && packet.Length == 8) || (data1 == 0x4C && packet.Length == 6))
{
if (data1 == 0x40)
{
for (int j = 0; j < 6; j++)
{
reader.wm_rand[9 - j] = (byte)(packet.Data[2 + j] & 0xff);
}
}
else if (data1 == 0x46)
{
for (int j = 6; j < 10; j++)
{
reader.wm_rand[9 - j] = (byte)(packet.Data[2 + (j - 6)] & 0xff);
}
for (int j = 0; j < 2; j++)
{
reader.wm_key[5 - j] = (byte)(packet.Data[6 + j] & 0xff);
}
}
else if (data1 == 0x4C)
{
for (int j = 2; j < 6; j++)
{
reader.wm_key[5 - j] = (byte)(packet.Data[j] & 0xff);
}
// generate decryption once all data is loaded
reader.wm_gentabs();
reader.isEncrypted = true;
}
}
}
else
{
if (reader.isControllerID && packet.Length == 7)
{
if ((packet.Data[packet.Length - 2] & 0xff) == 0 && (packet.Data[packet.Length - 1] & 0xff) == 0)
{
reader.controllerType = 0;
}
else if ((packet.Data[packet.Length - 2] & 0xff) == 1 && (packet.Data[packet.Length - 1] & 0xff) == 1)
{
reader.controllerType = 1;
}
else
{
reader.controllerType = 2;
}
reader.isControllerID = false;
}
else if (reader.isControllerPoll && (packet.Length == 7 || packet.Length == 9))
{
for (int i = 1; i < packet.Length; ++i)
{
if (reader.isEncrypted)
{
packet.UnencryptedData[i - 1] = (byte)((byte)((packet.Data[i] & 0xff) ^ reader.wm_sb[(i - 1) % 8]) + reader.wm_ft[(i - 1) % 8]);
}
else
{
packet.UnencryptedData[i - 1] = (byte)(packet.Data[i] & 0xff);
}
}
if (reader.controllerType == 0) // Nunchuck
{
var outState = new ControllerStateBuilder();
byte stickX = packet.UnencryptedData[0];
byte stickY = packet.UnencryptedData[1];
ushort aX = (ushort)((packet.UnencryptedData[2] << 2) | ((packet.UnencryptedData[5] & 0b0000000000001100) >> 2));
ushort ay = (ushort)((packet.UnencryptedData[3] << 2) | ((packet.UnencryptedData[5] & 0b0000000000110000) >> 2));
ushort az = (ushort)((packet.UnencryptedData[4] << 2) | ((packet.UnencryptedData[5] & 0b0000000011000000) >> 2));
outState.SetButton("c", (packet.UnencryptedData[5] & 0b00000010) == 0);
outState.SetButton("z", (packet.UnencryptedData[5] & 0b00000001) == 0);
outState.SetAnalog("stick_x", (stickX - 128.0f) / 128.0f);
outState.SetAnalog("stick_y", (stickY - 128.0f) / 128.0f);
outState.SetButton("disconnect", false);
reader.ControllerStateChanged?.Invoke(reader, outState.Build());
}
else if (reader.controllerType == 1) // Classic Controller
{
var outState = new ControllerStateBuilder();
byte rightTrigger = (byte)(packet.UnencryptedData[3] & 0b00011111);
byte leftTrigger = (byte)(((packet.UnencryptedData[3] & 0b11100000) >> 5) | ((packet.UnencryptedData[2] & 0b01100000) >> 2));
byte leftX = (byte)(packet.UnencryptedData[0] & 0b00111111);
byte leftY = (byte)(packet.UnencryptedData[1] & 0b00111111);
byte rightX = (byte)(((packet.UnencryptedData[2] & 0b10000000) >> 7) | ((packet.UnencryptedData[1] & 0b11000000) >> 5) | ((packet.UnencryptedData[0] & 0b11000000) >> 3));
byte rightY = (byte)(packet.UnencryptedData[2] & 0b00011111);
outState.SetButton("up", (packet.UnencryptedData[5] & ~0xFE) == 0);
outState.SetButton("right", (packet.UnencryptedData[4] & ~0x7F) == 0);
outState.SetButton("down", (packet.UnencryptedData[4] & ~0xBF) == 0);
outState.SetButton("left", (packet.UnencryptedData[5] & ~0xFD) == 0);
outState.SetButton("b", (packet.UnencryptedData[5] & ~0xBF) == 0);
outState.SetButton("a", (packet.UnencryptedData[5] & ~0xEF) == 0);
outState.SetButton("y", (packet.UnencryptedData[5] & 0b00100000) == 0);
outState.SetButton("x", (packet.UnencryptedData[5] & 0b00001000) == 0);
outState.SetButton("select", (packet.UnencryptedData[4] & ~0xEF) == 0);
outState.SetButton("home", (packet.UnencryptedData[4] & 0b00001000) == 0);
outState.SetButton("start", (packet.UnencryptedData[4] & ~0xFB) == 0);
outState.SetButton("l", (packet.UnencryptedData[4] & 0b00100000) == 0);
outState.SetButton("r", (packet.UnencryptedData[4] & 0b00000010) == 0);
outState.SetAnalog("l_trig", leftTrigger / 31.0f);
outState.SetAnalog("r_trig", rightTrigger / 31.0f);
outState.SetButton("zl", (packet.UnencryptedData[5] & 0b10000000) == 0);
outState.SetButton("zr", (packet.UnencryptedData[5] & 0b00000100) == 0);
outState.SetAnalog("lstick_x", (leftX - 32.0f) / 32.0f);
outState.SetAnalog("lstick_y", (leftY - 32.0f) / 32.0f);
outState.SetAnalog("rstick_x", (rightX - 15.0f) / 15.0f);
outState.SetAnalog("rstick_y", (rightY - 15.0f) / 15.0f);
outState.SetButton("disconnect", false);
reader.ControllerStateChanged?.Invoke(reader, outState.Build());
}
else
{
var outState = new ControllerStateBuilder();
outState.SetButton("disconnect", true);
reader.ControllerStateChanged?.Invoke(reader, outState.Build());
}
}
}
}
}
}
