From 5e30393647570c4188fd5f968de19c8f4ce6b632 Mon Sep 17 00:00:00 2001 From: Travis Nickles Date: Sun, 6 May 2018 02:13:11 -0500 Subject: [PATCH] Initial commit with ViGEm and UdpServer support --- DS4Windows/DS4Control/ControlService.cs | 320 +++++++++--- DS4Windows/DS4Control/ScpUtil.cs | 33 +- DS4Windows/DS4Control/UdpServer.cs | 631 ++++++++++++++++++++++++ DS4Windows/DS4Forms/DS4Form.cs | 4 +- DS4Windows/DS4Library/DS4Device.cs | 20 +- DS4Windows/DS4Library/DS4State.cs | 33 +- DS4Windows/DS4Windows.csproj | 4 + 7 files changed, 948 insertions(+), 97 deletions(-) create mode 100644 DS4Windows/DS4Control/UdpServer.cs diff --git a/DS4Windows/DS4Control/ControlService.cs b/DS4Windows/DS4Control/ControlService.cs index 3c4ff38..38a54f4 100644 --- a/DS4Windows/DS4Control/ControlService.cs +++ b/DS4Windows/DS4Control/ControlService.cs @@ -7,6 +7,9 @@ using System.Media; using System.Threading.Tasks; using static DS4Windows.Global; using System.Threading; +using Nefarius.ViGEm.Client; +using Nefarius.ViGEm.Client.Targets; +using Nefarius.ViGEm.Client.Targets.Xbox360; using Registry = Microsoft.Win32.Registry; namespace DS4Windows @@ -14,6 +17,10 @@ namespace DS4Windows public class ControlService { public X360Device x360Bus = null; + public ViGEmClient vigemTestClient = null; + private const int inputResolution = 127 - (-128); + private const float reciprocalInputResolution = 1 / (float)inputResolution; + private const int outputResolution = 32767 - (-32768); public const int DS4_CONTROLLER_COUNT = 4; public DS4Device[] DS4Controllers = new DS4Device[DS4_CONTROLLER_COUNT]; public Mouse[] touchPad = new Mouse[DS4_CONTROLLER_COUNT]; @@ -28,6 +35,10 @@ namespace DS4Windows bool[] buttonsdown = new bool[4] { false, false, false, false }; bool[] held = new bool[DS4_CONTROLLER_COUNT]; int[] oldmouse = new int[DS4_CONTROLLER_COUNT] { -1, -1, -1, -1 }; + public Xbox360Controller[] x360controls = new Xbox360Controller[4] { null, null, null, null }; + private Xbox360Report[] x360reports = new Xbox360Report[4] { new Xbox360Report(), new Xbox360Report(), + new Xbox360Report(), new Xbox360Report() + }; Thread tempThread; public List affectedDevs = new List() { @@ -39,6 +50,7 @@ namespace DS4Windows }; public bool suspending; //SoundPlayer sp = new SoundPlayer(); + private UdpServer _udpServer; private class X360Data { @@ -48,6 +60,75 @@ namespace DS4Windows private X360Data[] processingData = new X360Data[4]; + void GetPadDetailForIdx(int padIdx, ref DualShockPadMeta meta) + { + //meta = new DualShockPadMeta(); + meta.PadId = (byte) padIdx; + meta.Model = DsModel.DS4; + + var d = DS4Controllers[padIdx]; + if (d == null) + { + meta.PadMacAddress = null; + meta.PadState = DsState.Disconnected; + meta.ConnectionType = DsConnection.None; + meta.Model = DsModel.None; + meta.BatteryStatus = 0; + meta.IsActive = false; + + //return meta; + } + + bool isValidSerial = false; + //if (d.isValidSerial()) + //{ + string stringMac = d.getMacAddress(); + if (!string.IsNullOrEmpty(stringMac)) + { + stringMac = string.Join("", stringMac.Split(':')); + //stringMac = stringMac.Replace(":", "").Trim(); + meta.PadMacAddress = System.Net.NetworkInformation.PhysicalAddress.Parse(stringMac); + isValidSerial = d.isValidSerial(); + } + //} + + if (!isValidSerial) + { + //meta.PadMacAddress = null; + meta.PadState = DsState.Disconnected; + } + else + { + if (d.isSynced() || d.IsAlive()) + meta.PadState = DsState.Connected; + else + meta.PadState = DsState.Reserved; + } + + meta.ConnectionType = (d.getConnectionType() == ConnectionType.USB) ? DsConnection.Usb : DsConnection.Bluetooth; + meta.IsActive = !d.isDS4Idle(); + + if (d.isCharging() && d.getBattery() >= 100) + meta.BatteryStatus = DsBattery.Charged; + else + { + if (d.getBattery() >= 95) + meta.BatteryStatus = DsBattery.Full; + else if (d.getBattery() >= 70) + meta.BatteryStatus = DsBattery.High; + else if (d.getBattery() >= 50) + meta.BatteryStatus = DsBattery.Medium; + else if (d.getBattery() >= 20) + meta.BatteryStatus = DsBattery.Low; + else if (d.getBattery() >= 5) + meta.BatteryStatus = DsBattery.Dying; + else + meta.BatteryStatus = DsBattery.None; + } + + //return meta; + } + public ControlService() { //sp.Stream = Properties.Resources.EE; @@ -70,6 +151,8 @@ namespace DS4Windows PreviousState[i] = new DS4State(); ExposedState[i] = new DS4StateExposed(CurrentState[i]); } + + _udpServer = new UdpServer(GetPadDetailForIdx); } private void WarnExclusiveModeFailure(DS4Device device) @@ -137,14 +220,44 @@ namespace DS4Windows return unplugResult; } + private void startViGEm() + { + tempThread = new Thread(() => { try { vigemTestClient = new ViGEmClient(); } catch { } }); + tempThread.Priority = ThreadPriority.AboveNormal; + tempThread.IsBackground = true; + tempThread.Start(); + while (tempThread.IsAlive) + { + Thread.SpinWait(500); + } + } + + private void stopViGEm() + { + if (tempThread != null) + { + tempThread.Interrupt(); + tempThread.Join(); + tempThread = null; + } + + if (vigemTestClient != null) + { + vigemTestClient.Dispose(); + vigemTestClient = null; + } + } + public bool Start(object tempui, bool showlog = true) { - if (x360Bus.Open() && x360Bus.Start()) + startViGEm(); + if (vigemTestClient != null) + //if (x360Bus.Open() && x360Bus.Start()) { if (showlog) LogDebug(Properties.Resources.Starting); - LogDebug("Connection to Scp Virtual Bus established"); + LogDebug("Connection to ViGEm established"); DS4Devices.isExclusiveMode = getUseExclusiveMode(); if (showlog) @@ -191,19 +304,17 @@ namespace DS4Windows if (!getDInputOnly(i) && device.isSynced()) { - int xinputIndex = x360Bus.FirstController + i; - LogDebug("Plugging in X360 Controller #" + xinputIndex); - bool xinputResult = x360Bus.Plugin(i); - if (xinputResult) + //int xinputIndex = x360Bus.FirstController + i; + LogDebug("Plugging in X360 Controller #" + (i + 1)); + useDInputOnly[i] = false; + x360controls[i] = new Xbox360Controller(vigemTestClient); + x360controls[i].Connect(); + int devIndex = i; + x360controls[i].FeedbackReceived += (sender, args) => { - useDInputOnly[i] = false; - LogDebug("X360 Controller # " + xinputIndex + " connected"); - } - else - { - useDInputOnly[i] = true; - LogDebug("X360 Controller # " + xinputIndex + " failed. Using DInput only mode"); - } + setRumble(args.SmallMotor, args.LargeMotor, devIndex); + }; + LogDebug("X360 Controller # " + (i + 1) + " connected"); } int tempIdx = i; @@ -211,6 +322,20 @@ namespace DS4Windows { this.On_Report(sender, e, tempIdx); }; + + if (_udpServer != null) + { + EventHandler tempEvnt = (sender, args) => + { + DualShockPadMeta padDetail = new DualShockPadMeta(); + GetPadDetailForIdx(tempIdx, ref padDetail); + _udpServer.NewReportIncoming(ref padDetail, CurrentState[tempIdx]); + }; + + device.Report += tempEvnt; + device.MotionEvent = tempEvnt; + } + TouchPadOn(i, device); CheckProfileOptions(i, device, true); device.StartUpdate(); @@ -243,10 +368,28 @@ namespace DS4Windows } running = true; + + if (_udpServer != null) + { + var UDP_SERVER_PORT = 26760; + + try + { + _udpServer.Start(UDP_SERVER_PORT); + LogDebug("UDP server listening on port " + UDP_SERVER_PORT); + } + catch (System.Net.Sockets.SocketException ex) + { + var errMsg = String.Format("Couldn't start UDP server on port {0}, outside applications won't be able to access pad data ({1})", UDP_SERVER_PORT, ex.SocketErrorCode); + + LogDebug(errMsg, true); + Log.LogToTray(errMsg, true, true); + } + } } else { - string logMessage = "Could not connect to Scp Virtual Bus Driver. Please check the status of the System device in Device Manager"; + string logMessage = "Could not connect to ViGEm. Please check the status of the System device in Device Manager"; LogDebug(logMessage); Log.LogToTray(logMessage); } @@ -265,9 +408,8 @@ namespace DS4Windows if (showlog) LogDebug(Properties.Resources.StoppingX360); - LogDebug("Closing connection to Scp Virtual Bus"); + LogDebug("Closing connection to ViGEm"); - bool anyUnplugged = false; for (int i = 0, arlength = DS4Controllers.Length; i < arlength; i++) { DS4Device tempDevice = DS4Controllers[i]; @@ -299,9 +441,9 @@ namespace DS4Windows } CurrentState[i].Battery = PreviousState[i].Battery = 0; // Reset for the next connection's initial status change. - x360Bus.Unplug(i); + x360controls[i]?.Disconnect(); + x360controls[i] = null; useDInputOnly[i] = true; - anyUnplugged = true; DS4Controllers[i] = null; touchPad[i] = null; lag[i] = false; @@ -309,18 +451,18 @@ namespace DS4Windows } } - if (anyUnplugged) - Thread.Sleep(XINPUT_UNPLUG_SETTLE_TIME); - - x360Bus.UnplugAll(); - x360Bus.Stop(); - if (showlog) LogDebug(Properties.Resources.StoppingDS4); DS4Devices.stopControllers(); + + if (_udpServer != null) + _udpServer.Stop(); + if (showlog) LogDebug(Properties.Resources.StoppedDS4Windows); + + stopViGEm(); } runHotPlug = false; @@ -388,21 +530,33 @@ namespace DS4Windows { this.On_Report(sender, e, tempIdx); }; + + if (_udpServer != null) + { + EventHandler tempEvnt = (sender, args) => + { + DualShockPadMeta padDetail = new DualShockPadMeta(); + GetPadDetailForIdx(tempIdx, ref padDetail); + _udpServer.NewReportIncoming(ref padDetail, CurrentState[tempIdx]); + }; + + device.Report += tempEvnt; + device.MotionEvent = tempEvnt; + } + if (!getDInputOnly(Index) && device.isSynced()) { - int xinputIndex = x360Bus.FirstController + Index; - LogDebug("Plugging in X360 Controller #" + xinputIndex); - bool xinputResult = x360Bus.Plugin(Index); - if (xinputResult) + //int xinputIndex = x360Bus.FirstController + Index; + LogDebug("Plugging in X360 Controller #" + (Index + 1)); + useDInputOnly[Index] = false; + x360controls[Index] = new Xbox360Controller(vigemTestClient); + x360controls[Index].Connect(); + int devIndex = Index; + x360controls[Index].FeedbackReceived += (sender, args) => { - useDInputOnly[Index] = false; - LogDebug("X360 Controller # " + xinputIndex + " connected"); - } - else - { - useDInputOnly[Index] = true; - LogDebug("X360 Controller # " + xinputIndex + " failed. Using DInput only mode"); - } + setRumble(args.SmallMotor, args.LargeMotor, devIndex); + }; + LogDebug("X360 Controller # " + (Index + 1) + " connected"); } TouchPadOn(Index, device); @@ -432,6 +586,49 @@ namespace DS4Windows return true; } + private void testNewReport(ref Xbox360Report xboxreport, DS4State state) + { + Xbox360Buttons tempButtons = 0; + + if (state.Share) tempButtons |= Xbox360Buttons.Back; + if (state.L3) tempButtons |= Xbox360Buttons.LeftThumb; + if (state.R3) tempButtons |= Xbox360Buttons.RightThumb; + if (state.Options) tempButtons |= Xbox360Buttons.Start; + + if (state.DpadUp) tempButtons |= Xbox360Buttons.Up; + if (state.DpadRight) tempButtons |= Xbox360Buttons.Right; + if (state.DpadDown) tempButtons |= Xbox360Buttons.Down; + if (state.DpadLeft) tempButtons |= Xbox360Buttons.Left; + + if (state.L1) tempButtons |= Xbox360Buttons.LeftShoulder; + if (state.R1) tempButtons |= Xbox360Buttons.RightShoulder; + + if (state.Triangle) tempButtons |= Xbox360Buttons.Y; + if (state.Circle) tempButtons |= Xbox360Buttons.B; + if (state.Cross) tempButtons |= Xbox360Buttons.A; + if (state.Square) tempButtons |= Xbox360Buttons.X; + if (state.PS) tempButtons |= Xbox360Buttons.Guide; + xboxreport.SetButtons(tempButtons); + + xboxreport.LeftTrigger = state.L2; + xboxreport.RightTrigger = state.R2; + xboxreport.LeftThumbX = AxisScale(state.LX, false); + xboxreport.LeftThumbY = AxisScale(state.LY, true); + xboxreport.RightThumbX = AxisScale(state.RX, false); + xboxreport.RightThumbY = AxisScale(state.RY, true); + } + + private short AxisScale(Int32 Value, Boolean Flip) + { + Value -= 0x80; + + //float temp = (Value - (-128)) / (float)inputResolution; + float temp = (Value - (-128)) * reciprocalInputResolution; + if (Flip) temp = (temp - 0.5f) * -1.0f + 0.5f; + + return (short) (temp* outputResolution + (-32768)); + } + private void CheckProfileOptions(int ind, DS4Device device, bool startUp=false) { device.setIdleTimeout(getIdleDisconnectTimeout(ind)); @@ -647,36 +844,25 @@ namespace DS4Windows { if (!useDInputOnly[ind]) { - bool unplugResult = x360Bus.Unplug(ind); - int xinputIndex = x360Bus.FirstController + ind; - if (unplugResult) - { - useDInputOnly[ind] = true; - LogDebug("X360 Controller # " + xinputIndex + " unplugged"); - } - else - { - LogDebug("X360 Controller # " + xinputIndex + " failed to unplug"); - } + x360controls[ind].Disconnect(); + x360controls[ind] = null; + useDInputOnly[ind] = true; + LogDebug("X360 Controller # " + (ind + 1) + " unplugged"); } } else { if (!getDInputOnly(ind)) { - int xinputIndex = x360Bus.FirstController + ind; - LogDebug("Plugging in X360 Controller #" + xinputIndex); - bool xinputResult = x360Bus.Plugin(ind); - if (xinputResult) + LogDebug("Plugging in X360 Controller #" + (ind + 1)); + x360controls[ind] = new Xbox360Controller(vigemTestClient); + x360controls[ind].Connect(); + x360controls[ind].FeedbackReceived += (eventsender, args) => { - useDInputOnly[ind] = false; - LogDebug("X360 Controller # " + xinputIndex + " connected"); - } - else - { - useDInputOnly[ind] = true; - LogDebug("X360 Controller # " + xinputIndex + " failed. Using DInput only mode"); - } + setRumble(args.SmallMotor, args.LargeMotor, ind); + }; + useDInputOnly[ind] = false; + LogDebug("X360 Controller # " + (ind + 1) + " connected"); } } } @@ -710,9 +896,9 @@ namespace DS4Windows CurrentState[ind].Battery = PreviousState[ind].Battery = 0; // Reset for the next connection's initial status change. if (!useDInputOnly[ind]) { - bool unplugResult = x360Bus.Unplug(ind); - int xinputIndex = x360Bus.FirstController + ind; - LogDebug("X360 Controller # " + xinputIndex + " unplugged"); + x360controls[ind].Disconnect(); + x360controls[ind] = null; + LogDebug("X360 Controller # " + (ind + 1) + " unplugged"); } string removed = Properties.Resources.ControllerWasRemoved.Replace("*Mac address*", (ind + 1).ToString()); @@ -743,7 +929,6 @@ namespace DS4Windows inWarnMonitor[ind] = false; useDInputOnly[ind] = true; OnControllerRemoved(this, ind); - Thread.Sleep(XINPUT_UNPLUG_SETTLE_TIME); } } } @@ -840,10 +1025,12 @@ namespace DS4Windows if (!useDInputOnly[ind]) { - x360Bus.Parse(cState, processingData[ind].Report, ind); + testNewReport(ref x360reports[ind], cState); + x360controls[ind]?.SendReport(x360reports[ind]); + //x360Bus.Parse(cState, processingData[ind].Report, ind); // We push the translated Xinput state, and simultaneously we // pull back any possible rumble data coming from Xinput consumers. - if (x360Bus.Report(processingData[ind].Report, processingData[ind].Rumble)) + /*if (x360Bus.Report(processingData[ind].Report, processingData[ind].Rumble)) { byte Big = processingData[ind].Rumble[3]; byte Small = processingData[ind].Rumble[4]; @@ -853,6 +1040,7 @@ namespace DS4Windows setRumble(Big, Small, ind); } } + */ } // Output any synthetic events. diff --git a/DS4Windows/DS4Control/ScpUtil.cs b/DS4Windows/DS4Control/ScpUtil.cs index 10150d2..e67a646 100644 --- a/DS4Windows/DS4Control/ScpUtil.cs +++ b/DS4Windows/DS4Control/ScpUtil.cs @@ -315,9 +315,9 @@ namespace DS4Windows dataBuffer, dataBuffer.Length, ref requiredSize, 0)) { string hardwareId = dataBuffer.ToUTF16String(); - //if (hardwareIds.Contains("Scp Virtual Bus Driver")) + //if (hardwareIds.Contains("Virtual Gamepad Emulation Bus")) // result = true; - if (hardwareId.Equals(@"root\ScpVBus")) + if (hardwareId.Equals(@"Root\ViGEmBus")) result = true; } } @@ -2843,32 +2843,15 @@ namespace DS4Windows tempDev.setBTPollRate(btPollRate[device]); if (xinputStatus && xinputPlug) { - bool xinputResult = control.x360Bus.Plugin(device); - int xinputIndex = control.x360Bus.FirstController + device; - if (xinputResult) - { - dinputOnly[device] = false; - Log.LogToGui("X360 Controller # " + xinputIndex + " connected", false); - } - else - { - dinputOnly[device] = true; - Log.LogToGui("X360 Controller # " + xinputIndex + " failed. Using DInput only mode", true); - } + control.x360controls[device] = new Nefarius.ViGEm.Client.Targets.Xbox360Controller(control.vigemTestClient); + control.x360controls[device].Connect(); + Log.LogToGui("X360 Controller # " + (device + 1) + " connected", false); } else if (xinputStatus && !xinputPlug) { - bool xinputResult = control.x360Bus.Unplug(device); - int xinputIndex = control.x360Bus.FirstController + device; - if (xinputResult) - { - dinputOnly[device] = true; - Log.LogToGui("X360 Controller # " + xinputIndex + " unplugged", false); - } - else - { - Log.LogToGui("X360 Controller # " + xinputIndex + " failed to unplug", true); - } + control.x360controls[device].Disconnect(); + control.x360controls[device] = null; + Log.LogToGui("X360 Controller # " + (device + 1) + " unplugged", false); } tempDev.setRumble(0, 0); diff --git a/DS4Windows/DS4Control/UdpServer.cs b/DS4Windows/DS4Control/UdpServer.cs new file mode 100644 index 0000000..dfd4cfc --- /dev/null +++ b/DS4Windows/DS4Control/UdpServer.cs @@ -0,0 +1,631 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Net; +using System.Net.NetworkInformation; +using System.Net.Sockets; +using System.ComponentModel; + +namespace DS4Windows +{ + public enum DsState : byte + { + [Description("Disconnected")] + Disconnected = 0x00, + [Description("Reserved")] + Reserved = 0x01, + [Description("Connected")] + Connected = 0x02 + }; + + public enum DsConnection : byte + { + [Description("None")] + None = 0x00, + [Description("Usb")] + Usb = 0x01, + [Description("Bluetooth")] + Bluetooth = 0x02 + }; + + public enum DsModel : byte + { + [Description("None")] + None = 0, + [Description("DualShock 3")] + DS3 = 1, + [Description("DualShock 4")] + DS4 = 2, + [Description("Generic Gamepad")] + Generic = 3 + } + + public enum DsBattery : byte + { + None = 0x00, + Dying = 0x01, + Low = 0x02, + Medium = 0x03, + High = 0x04, + Full = 0x05, + Charging = 0xEE, + Charged = 0xEF + }; + + public struct DualShockPadMeta + { + public byte PadId; + public DsState PadState; + public DsConnection ConnectionType; + public DsModel Model; + public PhysicalAddress PadMacAddress; + public DsBattery BatteryStatus; + public bool IsActive; + } + + class UdpServer + { + private Socket udpSock; + private uint serverId; + private bool running; + private byte[] recvBuffer = new byte[1024]; + + public delegate void GetPadDetail(int padIdx, ref DualShockPadMeta meta); + + private GetPadDetail portInfoGet; + + public UdpServer(GetPadDetail getPadDetailDel) + { + portInfoGet = getPadDetailDel; + } + + enum MessageType + { + DSUC_VersionReq = 0x100000, + DSUS_VersionRsp = 0x100000, + DSUC_ListPorts = 0x100001, + DSUS_PortInfo = 0x100001, + DSUC_PadDataReq = 0x100002, + DSUS_PadDataRsp = 0x100002, + }; + + private const ushort MaxProtocolVersion = 1001; + + class ClientRequestTimes + { + DateTime allPads; + DateTime[] padIds; + Dictionary padMacs; + + public DateTime AllPadsTime { get { return allPads; } } + public DateTime[] PadIdsTime { get { return padIds; } } + public Dictionary PadMacsTime { get { return padMacs; } } + + public ClientRequestTimes() + { + allPads = DateTime.MinValue; + padIds = new DateTime[4]; + + for (int i = 0; i < padIds.Length; i++) + padIds[i] = DateTime.MinValue; + + padMacs = new Dictionary(); + } + + public void RequestPadInfo(byte regFlags, byte idToReg, PhysicalAddress macToReg) + { + if (regFlags == 0) + allPads = DateTime.UtcNow; + else + { + if ((regFlags & 0x01) != 0) //id valid + { + if (idToReg < padIds.Length) + padIds[idToReg] = DateTime.UtcNow; + } + if ((regFlags & 0x02) != 0) //mac valid + { + padMacs[macToReg] = DateTime.UtcNow; + } + } + } + } + + private Dictionary clients = new Dictionary(); + + private int BeginPacket(byte[] packetBuf, ushort reqProtocolVersion = MaxProtocolVersion) + { + int currIdx = 0; + packetBuf[currIdx++] = (byte)'D'; + packetBuf[currIdx++] = (byte)'S'; + packetBuf[currIdx++] = (byte)'U'; + packetBuf[currIdx++] = (byte)'S'; + + Array.Copy(BitConverter.GetBytes((ushort)reqProtocolVersion), 0, packetBuf, currIdx, 2); + currIdx += 2; + + Array.Copy(BitConverter.GetBytes((ushort)packetBuf.Length - 16), 0, packetBuf, currIdx, 2); + currIdx += 2; + + Array.Clear(packetBuf, currIdx, 4); //place for crc + currIdx += 4; + + Array.Copy(BitConverter.GetBytes((uint)serverId), 0, packetBuf, currIdx, 4); + currIdx += 4; + + return currIdx; + } + + private void FinishPacket(byte[] packetBuf) + { + Array.Clear(packetBuf, 8, 4); + + //uint crcCalc = Crc32Algorithm.Compute(packetBuf); + uint seed = Crc32Algorithm.DefaultSeed; + uint crcCalc = ~Crc32Algorithm.CalculateBasicHash(ref seed, ref packetBuf, 0, packetBuf.Length); + Array.Copy(BitConverter.GetBytes((uint)crcCalc), 0, packetBuf, 8, 4); + } + + private void SendPacket(IPEndPoint clientEP, byte[] usefulData, ushort reqProtocolVersion = MaxProtocolVersion) + { + byte[] packetData = new byte[usefulData.Length + 16]; + int currIdx = BeginPacket(packetData, reqProtocolVersion); + Array.Copy(usefulData, 0, packetData, currIdx, usefulData.Length); + FinishPacket(packetData); + + try { udpSock.SendTo(packetData, clientEP); } + catch (Exception e) { } + } + + private void ProcessIncoming(byte[] localMsg, IPEndPoint clientEP) + { + try + { + int currIdx = 0; + if (localMsg[0] != 'D' || localMsg[1] != 'S' || localMsg[2] != 'U' || localMsg[3] != 'C') + return; + else + currIdx += 4; + + uint protocolVer = BitConverter.ToUInt16(localMsg, currIdx); + currIdx += 2; + + if (protocolVer > MaxProtocolVersion) + return; + + uint packetSize = BitConverter.ToUInt16(localMsg, currIdx); + currIdx += 2; + + if (packetSize < 0) + return; + + packetSize += 16; //size of header + if (packetSize > localMsg.Length) + return; + else if (packetSize < localMsg.Length) + { + byte[] newMsg = new byte[packetSize]; + Array.Copy(localMsg, newMsg, packetSize); + localMsg = newMsg; + } + + uint crcValue = BitConverter.ToUInt32(localMsg, currIdx); + //zero out the crc32 in the packet once we got it since that's whats needed for calculation + localMsg[currIdx++] = 0; + localMsg[currIdx++] = 0; + localMsg[currIdx++] = 0; + localMsg[currIdx++] = 0; + + uint crcCalc = Crc32Algorithm.Compute(localMsg); + if (crcValue != crcCalc) + return; + + uint clientId = BitConverter.ToUInt32(localMsg, currIdx); + currIdx += 4; + + uint messageType = BitConverter.ToUInt32(localMsg, currIdx); + currIdx += 4; + + if (messageType == (uint)MessageType.DSUC_VersionReq) + { + byte[] outputData = new byte[8]; + int outIdx = 0; + Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_VersionRsp), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes((ushort)MaxProtocolVersion), 0, outputData, outIdx, 2); + outIdx += 2; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + + SendPacket(clientEP, outputData, 1001); + } + else if (messageType == (uint)MessageType.DSUC_ListPorts) + { + int numPadRequests = BitConverter.ToInt32(localMsg, currIdx); + currIdx += 4; + if (numPadRequests < 0 || numPadRequests > 4) + return; + + int requestsIdx = currIdx; + for (int i = 0; i < numPadRequests; i++) + { + byte currRequest = localMsg[requestsIdx + i]; + if (currRequest >= 4) + return; + } + + byte[] outputData = new byte[16]; + for (byte i = 0; i < numPadRequests; i++) + { + byte currRequest = localMsg[requestsIdx + i]; + DualShockPadMeta padData = new DualShockPadMeta(); + portInfoGet(currRequest, ref padData); + + int outIdx = 0; + Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_PortInfo), 0, outputData, outIdx, 4); + outIdx += 4; + + outputData[outIdx++] = (byte)padData.PadId; + outputData[outIdx++] = (byte)padData.PadState; + outputData[outIdx++] = (byte)padData.Model; + outputData[outIdx++] = (byte)padData.ConnectionType; + + byte[] addressBytes = null; + if (padData.PadMacAddress != null) + addressBytes = padData.PadMacAddress.GetAddressBytes(); + + if (addressBytes != null && addressBytes.Length == 6) + { + outputData[outIdx++] = addressBytes[0]; + outputData[outIdx++] = addressBytes[1]; + outputData[outIdx++] = addressBytes[2]; + outputData[outIdx++] = addressBytes[3]; + outputData[outIdx++] = addressBytes[4]; + outputData[outIdx++] = addressBytes[5]; + } + else + { + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + outputData[outIdx++] = 0; + } + + outputData[outIdx++] = (byte)padData.BatteryStatus; + outputData[outIdx++] = 0; + + SendPacket(clientEP, outputData, 1001); + } + } + else if (messageType == (uint)MessageType.DSUC_PadDataReq) + { + byte regFlags = localMsg[currIdx++]; + byte idToReg = localMsg[currIdx++]; + PhysicalAddress macToReg = null; + { + byte[] macBytes = new byte[6]; + Array.Copy(localMsg, currIdx, macBytes, 0, macBytes.Length); + currIdx += macBytes.Length; + macToReg = new PhysicalAddress(macBytes); + } + + lock (clients) + { + if (clients.ContainsKey(clientEP)) + clients[clientEP].RequestPadInfo(regFlags, idToReg, macToReg); + else + { + var clientTimes = new ClientRequestTimes(); + clientTimes.RequestPadInfo(regFlags, idToReg, macToReg); + clients[clientEP] = clientTimes; + } + } + } + } + catch (Exception e) { } + } + + private void ReceiveCallback(IAsyncResult iar) + { + byte[] localMsg = null; + EndPoint clientEP = new IPEndPoint(IPAddress.Any, 0); + + try + { + //Get the received message. + Socket recvSock = (Socket)iar.AsyncState; + int msgLen = recvSock.EndReceiveFrom(iar, ref clientEP); + + localMsg = new byte[msgLen]; + Array.Copy(recvBuffer, localMsg, msgLen); + } + catch (Exception e) { } + + //Start another receive as soon as we copied the data + StartReceive(); + + //Process the data if its valid + if (localMsg != null) + ProcessIncoming(localMsg, (IPEndPoint)clientEP); + } + private void StartReceive() + { + try + { + if (running) + { + //Start listening for a new message. + EndPoint newClientEP = new IPEndPoint(IPAddress.Any, 0); + udpSock.BeginReceiveFrom(recvBuffer, 0, recvBuffer.Length, SocketFlags.None, ref newClientEP, ReceiveCallback, udpSock); + } + } + catch (SocketException ex) + { + uint IOC_IN = 0x80000000; + uint IOC_VENDOR = 0x18000000; + uint SIO_UDP_CONNRESET = IOC_IN | IOC_VENDOR | 12; + udpSock.IOControl((int)SIO_UDP_CONNRESET, new byte[] { Convert.ToByte(false) }, null); + + StartReceive(); + } + } + + public void Start(int port) + { + if (running) + { + if (udpSock != null) + { + udpSock.Close(); + udpSock = null; + } + running = false; + } + + udpSock = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp); + try { udpSock.Bind(new IPEndPoint(IPAddress.Loopback, port)); } + catch (SocketException ex) + { + udpSock.Close(); + udpSock = null; + + throw ex; + } + + byte[] randomBuf = new byte[4]; + new Random().NextBytes(randomBuf); + serverId = BitConverter.ToUInt32(randomBuf, 0); + + running = true; + StartReceive(); + } + + public void Stop() + { + running = false; + if (udpSock != null) + { + udpSock.Close(); + udpSock = null; + } + } + + private bool ReportToBuffer(DS4State hidReport, byte[] outputData, ref int outIdx) + { + outputData[outIdx] = 0; + + if (hidReport.DpadLeft) outputData[outIdx] |= 0x80; + if (hidReport.DpadDown) outputData[outIdx] |= 0x40; + if (hidReport.DpadRight) outputData[outIdx] |= 0x20; + if (hidReport.DpadUp) outputData[outIdx] |= 0x10; + + if (hidReport.Options) outputData[outIdx] |= 0x08; + if (hidReport.R3) outputData[outIdx] |= 0x04; + if (hidReport.L3) outputData[outIdx] |= 0x02; + if (hidReport.Share) outputData[outIdx] |= 0x01; + + outputData[++outIdx] = 0; + + if (hidReport.Square) outputData[outIdx] |= 0x80; + if (hidReport.Cross) outputData[outIdx] |= 0x40; + if (hidReport.Circle) outputData[outIdx] |= 0x20; + if (hidReport.Triangle) outputData[outIdx] |= 0x10; + + if (hidReport.R1) outputData[outIdx] |= 0x08; + if (hidReport.L1) outputData[outIdx] |= 0x04; + if (hidReport.R2Btn) outputData[outIdx] |= 0x02; + if (hidReport.L2Btn) outputData[outIdx] |= 0x01; + + outputData[++outIdx] = (hidReport.PS) ? (byte)1 : (byte)0; + outputData[++outIdx] = (hidReport.TouchButton) ? (byte)1 : (byte)0; + + //Left stick + outputData[++outIdx] = hidReport.LX; + outputData[++outIdx] = hidReport.LY; + outputData[outIdx] = (byte)(255 - outputData[outIdx]); //invert Y by convention + + //Right stick + outputData[++outIdx] = hidReport.RX; + outputData[++outIdx] = hidReport.RY; + outputData[outIdx] = (byte)(255 - outputData[outIdx]); //invert Y by convention + + //we don't have analog buttons on DS4 :( + outputData[++outIdx] = hidReport.DpadLeft ? (byte)0xFF : (byte)0x00; + outputData[++outIdx] = hidReport.DpadDown ? (byte)0xFF : (byte)0x00; + outputData[++outIdx] = hidReport.DpadRight ? (byte)0xFF : (byte)0x00; + outputData[++outIdx] = hidReport.DpadUp ? (byte)0xFF : (byte)0x00; + + outputData[++outIdx] = hidReport.Square ? (byte)0xFF : (byte)0x00; + outputData[++outIdx] = hidReport.Cross ? (byte)0xFF : (byte)0x00; + outputData[++outIdx] = hidReport.Circle ? (byte)0xFF : (byte)0x00; + outputData[++outIdx] = hidReport.Triangle ? (byte)0xFF : (byte)0x00; + + outputData[++outIdx] = hidReport.R1 ? (byte)0xFF : (byte)0x00; + outputData[++outIdx] = hidReport.L1 ? (byte)0xFF : (byte)0x00; + + outputData[++outIdx] = hidReport.R2; + outputData[++outIdx] = hidReport.L2; + + outIdx++; + + //DS4 only: touchpad points + for (int i = 0; i < 2; i++) + { + var tpad = (i == 0) ? hidReport.TrackPadTouch0 : hidReport.TrackPadTouch1; + + outputData[outIdx++] = tpad.IsActive ? (byte)1 : (byte)0; + outputData[outIdx++] = (byte)tpad.Id; + Array.Copy(BitConverter.GetBytes((ushort)tpad.X), 0, outputData, outIdx, 2); + outIdx += 2; + Array.Copy(BitConverter.GetBytes((ushort)tpad.Y), 0, outputData, outIdx, 2); + outIdx += 2; + } + + //motion timestamp + if (hidReport.Motion != null) + Array.Copy(BitConverter.GetBytes((ulong)hidReport.totalMicroSec), 0, outputData, outIdx, 8); + else + Array.Clear(outputData, outIdx, 8); + + outIdx += 8; + + //accelerometer + if (hidReport.Motion != null) + { + Array.Copy(BitConverter.GetBytes((float)hidReport.Motion.accelXG), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes((float)hidReport.Motion.accelYG), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes((float)-hidReport.Motion.accelZG), 0, outputData, outIdx, 4); + outIdx += 4; + } + else + { + Array.Clear(outputData, outIdx, 12); + outIdx += 12; + } + + //gyroscope + if (hidReport.Motion != null) + { + Array.Copy(BitConverter.GetBytes((float)hidReport.Motion.angVelPitch), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes((float)hidReport.Motion.angVelYaw), 0, outputData, outIdx, 4); + outIdx += 4; + Array.Copy(BitConverter.GetBytes((float)hidReport.Motion.angVelRoll), 0, outputData, outIdx, 4); + outIdx += 4; + } + else + { + Array.Clear(outputData, outIdx, 12); + outIdx += 12; + } + + return true; + } + + public void NewReportIncoming(ref DualShockPadMeta padMeta, DS4State hidReport) + { + if (!running) + return; + + var clientsList = new List(); + var now = DateTime.UtcNow; + lock (clients) + { + var clientsToDelete = new List(); + + foreach (var cl in clients) + { + const double TimeoutLimit = 5; + + if ((now - cl.Value.AllPadsTime).TotalSeconds < TimeoutLimit) + clientsList.Add(cl.Key); + else if ((padMeta.PadId < cl.Value.PadIdsTime.Length) && + (now - cl.Value.PadIdsTime[(byte)padMeta.PadId]).TotalSeconds < TimeoutLimit) + clientsList.Add(cl.Key); + else if (cl.Value.PadMacsTime.ContainsKey(padMeta.PadMacAddress) && + (now - cl.Value.PadMacsTime[padMeta.PadMacAddress]).TotalSeconds < TimeoutLimit) + clientsList.Add(cl.Key); + else //check if this client is totally dead, and remove it if so + { + bool clientOk = false; + for (int i = 0; i < cl.Value.PadIdsTime.Length; i++) + { + var dur = (now - cl.Value.PadIdsTime[i]).TotalSeconds; + if (dur < TimeoutLimit) + { + clientOk = true; + break; + } + } + if (!clientOk) + { + foreach (var dict in cl.Value.PadMacsTime) + { + var dur = (now - dict.Value).TotalSeconds; + if (dur < TimeoutLimit) + { + clientOk = true; + break; + } + } + + if (!clientOk) + clientsToDelete.Add(cl.Key); + } + } + } + + foreach (var delCl in clientsToDelete) + { + clients.Remove(delCl); + } + clientsToDelete.Clear(); + clientsToDelete = null; + } + + if (clientsList.Count <= 0) + return; + + byte[] outputData = new byte[100]; + int outIdx = BeginPacket(outputData, 1001); + Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_PadDataRsp), 0, outputData, outIdx, 4); + outIdx += 4; + + outputData[outIdx++] = (byte)padMeta.PadId; + outputData[outIdx++] = (byte)padMeta.PadState; + outputData[outIdx++] = (byte)padMeta.Model; + outputData[outIdx++] = (byte)padMeta.ConnectionType; + { + byte[] padMac = padMeta.PadMacAddress.GetAddressBytes(); + outputData[outIdx++] = padMac[0]; + outputData[outIdx++] = padMac[1]; + outputData[outIdx++] = padMac[2]; + outputData[outIdx++] = padMac[3]; + outputData[outIdx++] = padMac[4]; + outputData[outIdx++] = padMac[5]; + } + outputData[outIdx++] = (byte)padMeta.BatteryStatus; + outputData[outIdx++] = padMeta.IsActive ? (byte)1 : (byte)0; + + Array.Copy(BitConverter.GetBytes((uint)hidReport.PacketCounter), 0, outputData, outIdx, 4); + outIdx += 4; + + if (!ReportToBuffer(hidReport, outputData, ref outIdx)) + return; + else + FinishPacket(outputData); + + foreach (var cl in clientsList) + { + try { udpSock.SendTo(outputData, cl); } + catch (SocketException ex) { } + } + clientsList.Clear(); + clientsList = null; + } + } +} diff --git a/DS4Windows/DS4Forms/DS4Form.cs b/DS4Windows/DS4Forms/DS4Form.cs index 7265cc1..c67297f 100644 --- a/DS4Windows/DS4Forms/DS4Form.cs +++ b/DS4Windows/DS4Forms/DS4Form.cs @@ -202,7 +202,7 @@ namespace DS4Windows cBDisconnectBT.Checked = DCBTatStop; cBQuickCharge.Checked = QuickCharge; nUDXIPorts.Value = FirstXinputPort; - Program.rootHub.x360Bus.FirstController = FirstXinputPort; + //Program.rootHub.x360Bus.FirstController = FirstXinputPort; // New settings this.Width = FormWidth; this.Height = FormHeight; @@ -2185,7 +2185,7 @@ Properties.Resources.DS4Update, MessageBoxButtons.YesNo, MessageBoxIcon.Question { oldxiport = (int)Math.Round(nUDXIPorts.Value, 0); FirstXinputPort = oldxiport; - Program.rootHub.x360Bus.FirstController = oldxiport; + //Program.rootHub.x360Bus.FirstController = oldxiport; BtnStartStop_Clicked(false); finishHideDS4Check(); } diff --git a/DS4Windows/DS4Library/DS4Device.cs b/DS4Windows/DS4Library/DS4Device.cs index 9ebf68d..30d2923 100644 --- a/DS4Windows/DS4Library/DS4Device.cs +++ b/DS4Windows/DS4Library/DS4Device.cs @@ -164,6 +164,7 @@ namespace DS4Windows public event EventHandler Removal = null; public event EventHandler SyncChange = null; public event EventHandler SerialChange = null; + public EventHandler MotionEvent = null; public HidDevice HidDevice => hDevice; public bool IsExclusive => HidDevice.IsExclusive; @@ -774,7 +775,7 @@ namespace DS4Windows //Console.WriteLine(MacAddress.ToString() + " " + System.DateTime.UtcNow.ToString("o") + "" + // "> invalid CRC32 in BT input report: 0x" + recvCrc32.ToString("X8") + " expected: 0x" + calcCrc32.ToString("X8")); - //cState.PacketCounter = pState.PacketCounter + 1; //still increase so we know there were lost packets + cState.PacketCounter = pState.PacketCounter + 1; //still increase so we know there were lost packets continue; } } @@ -857,6 +858,7 @@ namespace DS4Windows utcNow = DateTime.UtcNow; // timestamp with UTC in case system time zone changes resetHapticState(); + cState.PacketCounter = pState.PacketCounter + 1; cState.ReportTimeStamp = utcNow; cState.LX = inputReport[1]; cState.LY = inputReport[2]; @@ -894,6 +896,8 @@ namespace DS4Windows cState.L3 = (tempByte & (1 << 6)) != 0; cState.Options = (tempByte & (1 << 5)) != 0; cState.Share = (tempByte & (1 << 4)) != 0; + cState.R2Btn = (inputReport[6] & (1 << 3)) != 0; + cState.L2Btn = (inputReport[6] & (1 << 2)) != 0; cState.R1 = (tempByte & (1 << 1)) != 0; cState.L1 = (tempByte & (1 << 0)) != 0; @@ -935,6 +939,18 @@ namespace DS4Windows timeStampPrevious = tempStamp; elapsedDeltaTime = 0.000001 * deltaTimeCurrent; // Convert from microseconds to seconds cState.elapsedTime = elapsedDeltaTime; + cState.totalMicroSec = pState.totalMicroSec + deltaTimeCurrent; + + //Simpler touch storing + cState.TrackPadTouch0.Id = (byte)(inputReport[35] & 0x7f); + cState.TrackPadTouch0.IsActive = (inputReport[35] & 0x80) == 0; + cState.TrackPadTouch0.X = (short)(((ushort)(inputReport[37] & 0x0f) << 8) | (ushort)(inputReport[36])); + cState.TrackPadTouch0.Y = (short)(((ushort)(inputReport[38]) << 4) | ((ushort)(inputReport[37] & 0xf0) >> 4)); + + cState.TrackPadTouch1.Id = (byte)(inputReport[39] & 0x7f); + cState.TrackPadTouch1.IsActive = (inputReport[39] & 0x80) == 0; + cState.TrackPadTouch1.X = (short)(((ushort)(inputReport[41] & 0x0f) << 8) | (ushort)(inputReport[40])); + cState.TrackPadTouch1.Y = (short)(((ushort)(inputReport[42]) << 4) | ((ushort)(inputReport[41] & 0xf0) >> 4)); // XXX DS4State mapping needs fixup, turn touches into an array[4] of structs. And include the touchpad details there instead. try @@ -1367,7 +1383,7 @@ namespace DS4Windows return pState; } - private bool isDS4Idle() + public bool isDS4Idle() { if (cState.Square || cState.Cross || cState.Circle || cState.Triangle) return false; diff --git a/DS4Windows/DS4Library/DS4State.cs b/DS4Windows/DS4Library/DS4State.cs index 66982eb..2978c2c 100644 --- a/DS4Windows/DS4Library/DS4State.cs +++ b/DS4Windows/DS4Library/DS4State.cs @@ -4,10 +4,11 @@ namespace DS4Windows { public class DS4State { + public uint PacketCounter; public DateTime ReportTimeStamp; public bool Square, Triangle, Circle, Cross; public bool DpadUp, DpadDown, DpadLeft, DpadRight; - public bool L1, L3, R1, R3; + public bool L1, L2Btn, L3, R1, R2Btn, R3; public bool Share, Options, PS, Touch1, Touch2, TouchButton, TouchRight, TouchLeft, Touch1Finger, Touch2Fingers; public byte Touch1Identifier, Touch2Identifier; @@ -24,14 +25,27 @@ namespace DS4Windows public double RXUnit; public double RYUnit; public double elapsedTime = 0.0; + public ulong totalMicroSec = 0; public SixAxis Motion = null; public static readonly int DEFAULT_AXISDIR_VALUE = 127; + public struct TrackPadTouch + { + public bool IsActive; + public byte Id; + public short X; + public short Y; + } + + public TrackPadTouch TrackPadTouch0; + public TrackPadTouch TrackPadTouch1; + public DS4State() { + PacketCounter = 0; Square = Triangle = Circle = Cross = false; DpadUp = DpadDown = DpadLeft = DpadRight = false; - L1 = L3 = R1 = R3 = false; + L1 = L2Btn = L3 = R1 = R2Btn = R3 = false; Share = Options = PS = Touch1 = Touch2 = TouchButton = TouchRight = TouchLeft = false; Touch1Finger = Touch2Fingers = false; LX = RX = LY = RY = 127; @@ -48,11 +62,15 @@ namespace DS4Windows RXUnit = 0.0; RYUnit = 0.0; elapsedTime = 0.0; + totalMicroSec = 0; Motion = new SixAxis(0, 0, 0, 0, 0, 0, 0.0); + TrackPadTouch0.IsActive = false; + TrackPadTouch1.IsActive = false; } public DS4State(DS4State state) { + PacketCounter = state.PacketCounter; ReportTimeStamp = state.ReportTimeStamp; Square = state.Square; Triangle = state.Triangle; @@ -64,9 +82,11 @@ namespace DS4Windows DpadRight = state.DpadRight; L1 = state.L1; L2 = state.L2; + L2Btn = state.L2Btn; L3 = state.L3; R1 = state.R1; R2 = state.R2; + R2Btn = state.R2Btn; R3 = state.R3; Share = state.Share; Options = state.Options; @@ -96,7 +116,10 @@ namespace DS4Windows RXUnit = state.RXUnit; RYUnit = state.RYUnit; elapsedTime = state.elapsedTime; + totalMicroSec = state.totalMicroSec; Motion = state.Motion; + TrackPadTouch0 = state.TrackPadTouch0; + TrackPadTouch1 = state.TrackPadTouch1; } public DS4State Clone() @@ -106,6 +129,7 @@ namespace DS4Windows public void CopyTo(DS4State state) { + state.PacketCounter = PacketCounter; state.ReportTimeStamp = ReportTimeStamp; state.Square = Square; state.Triangle = Triangle; @@ -117,9 +141,11 @@ namespace DS4Windows state.DpadRight = DpadRight; state.L1 = L1; state.L2 = L2; + state.L2Btn = L2Btn; state.L3 = L3; state.R1 = R1; state.R2 = R2; + state.R2Btn = R2Btn; state.R3 = R3; state.Share = Share; state.Options = Options; @@ -149,7 +175,10 @@ namespace DS4Windows state.RXUnit = RXUnit; state.RYUnit = RYUnit; state.elapsedTime = elapsedTime; + state.totalMicroSec = totalMicroSec; state.Motion = Motion; + state.TrackPadTouch0 = TrackPadTouch0; + state.TrackPadTouch1 = TrackPadTouch1; } public void calculateStickAngles() diff --git a/DS4Windows/DS4Windows.csproj b/DS4Windows/DS4Windows.csproj index ba5bb6b..81c6315 100644 --- a/DS4Windows/DS4Windows.csproj +++ b/DS4Windows/DS4Windows.csproj @@ -123,6 +123,9 @@ ..\packages\TaskScheduler.2.8.0\lib\net452\Microsoft.Win32.TaskScheduler.dll + + ..\bin\Nefarius.ViGEmClient\Nefarius.ViGEmClient.dll + @@ -158,6 +161,7 @@ ScpHub.cs + Component