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()); } } } } } }