From 9fa0a7f1da1b70bee995f53c6c96c43189018772 Mon Sep 17 00:00:00 2001 From: mrb0nk500 Date: Wed, 1 Feb 2023 18:45:02 -0400 Subject: global: Import Dolphin SDK This version comes from the Metroid Prime decompilation project. https://github.com/PrimeDecomp/prime --- src/Dolphin/pad/PadClamp.c | 166 +++++++++ src/Dolphin/pad/pad.c | 838 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 1004 insertions(+) create mode 100644 src/Dolphin/pad/PadClamp.c create mode 100644 src/Dolphin/pad/pad.c (limited to 'src/Dolphin/pad') diff --git a/src/Dolphin/pad/PadClamp.c b/src/Dolphin/pad/PadClamp.c new file mode 100644 index 0000000..6b992ec --- /dev/null +++ b/src/Dolphin/pad/PadClamp.c @@ -0,0 +1,166 @@ +#include + +#include + +typedef struct PADClampRegion { + u8 minTrigger; + u8 maxTrigger; + s8 minStick; + s8 maxStick; + s8 xyStick; + s8 minSubstick; + s8 maxSubstick; + s8 xySubstick; + s8 radStick; + s8 radSubstick; +} PADClampRegion; + +static const PADClampRegion ClampRegion = { + // Triggers + 30, + 180, + + // Left stick + 15, + 72, + 40, + + // Right stick + 15, + 59, + 31, + + // Stick radii + 56, + 44, +}; + +static void ClampCircle(s8* px, s8* py, s8 radius, s8 min) { + s32 x = *px; + s32 y = *py; + s32 squared; + s32 length; + + if (-min < x && x < min) { + x = 0; + } else if (0 < x) { + x -= min; + } else { + x += min; + } + + if (-min < y && y < min) { + y = 0; + } else if (0 < y) { + y -= min; + } else { + y += min; + } + + squared = x * x + y * y; + if (radius * radius < squared) { + length = sqrt(squared); + x = (x * radius) / length; + y = (y * radius) / length; + } + + *px = x; + *py = y; +} + +static void ClampStick(s8* px, s8* py, s8 max, s8 xy, s8 min) { + int x = *px; + int y = *py; + int signX; + int signY; + int d; + + if (0 <= x) { + signX = 1; + } else { + signX = -1; + x = -x; + } + + if (0 <= y) { + signY = 1; + } else { + signY = -1; + y = -y; + } + + if (x <= min) { + x = 0; + } else { + x -= min; + } + if (y <= min) { + y = 0; + } else { + y -= min; + } + + if (x == 0 && y == 0) { + *px = *py = 0; + return; + } + + if (xy * y <= xy * x) { + d = xy * x + (max - xy) * y; + if (xy * max < d) { + x = (s8)(xy * max * x / d); + y = (s8)(xy * max * y / d); + } + } else { + d = xy * y + (max - xy) * x; + if (xy * max < d) { + x = (s8)(xy * max * x / d); + y = (s8)(xy * max * y / d); + } + } + + *px = (s8)(signX * x); + *py = (s8)(signY * y); +} + +static void ClampTrigger(u8* trigger, u8 min, u8 max) { + if (*trigger <= min) { + *trigger = 0; + } else { + if (max < *trigger) { + *trigger = max; + } + *trigger -= min; + } +} + +void PADClamp(PADStatus* status) { + int i; + for (i = 0; i < PAD_CHANMAX; i++, status++) { + if (status->err != PAD_ERR_NONE) { + continue; + } + + ClampStick(&status->stickX, &status->stickY, ClampRegion.maxStick, ClampRegion.xyStick, + ClampRegion.minStick); + ClampStick(&status->substickX, &status->substickY, ClampRegion.maxSubstick, + ClampRegion.xySubstick, ClampRegion.minSubstick); + ClampTrigger(&status->triggerL, ClampRegion.minTrigger, ClampRegion.maxTrigger); + ClampTrigger(&status->triggerR, ClampRegion.minTrigger, ClampRegion.maxTrigger); + } +} + +void PADClampCircle(PADStatus* status) { + u32 i; + for (i = 0; i < 4; ++i, status++) { + if (status->err != PAD_ERR_NONE) { + continue; + } + + ClampCircle(&status->stickX, &status->stickY, ClampRegion.radStick, ClampRegion.minStick); + ClampCircle(&status->substickX, &status->substickY, ClampRegion.radSubstick, + ClampRegion.minSubstick); + ClampTrigger(&status->triggerL, ClampRegion.minTrigger, ClampRegion.maxTrigger); + ClampTrigger(&status->triggerR, ClampRegion.minTrigger, ClampRegion.maxTrigger); + } +} diff --git a/src/Dolphin/pad/pad.c b/src/Dolphin/pad/pad.c new file mode 100644 index 0000000..3ccf7d1 --- /dev/null +++ b/src/Dolphin/pad/pad.c @@ -0,0 +1,838 @@ +#include +#include + +const char* __PADVersion = "<< Dolphin SDK - PAD\trelease build: Sep 5 2002 05:34:02 (0x2301) >>"; + +u8 UnkVal : (OS_BASE_CACHED | 0x30e3); +u16 __OSWirelessPadFixMode : (OS_BASE_CACHED | 0x30E0); + +static void PADTypeAndStatusCallback(s32 chan, u32 type); +static void PADOriginCallback(s32 chan, u32 error, OSContext* context); +static void PADProbeCallback(s32 chan, u32 error, OSContext* context); +static void SPEC0_MakeStatus(s32 chan, PADStatus* status, u32 data[2]); +static void SPEC1_MakeStatus(s32 chan, PADStatus* status, u32 data[2]); +static void SPEC2_MakeStatus(s32 chan, PADStatus* status, u32 data[2]); +static void PADTypeAndStatusCallback(s32 chan, u32 type); + +static void PADOriginCallback(s32 chan, u32 error, OSContext* context); +static void PADProbeCallback(s32 chan, u32 error, OSContext* context); + +static void SPEC0_MakeStatus(s32 chan, PADStatus* status, u32 data[2]); +static void SPEC1_MakeStatus(s32 chan, PADStatus* status, u32 data[2]); +static void SPEC2_MakeStatus(s32 chan, PADStatus* status, u32 data[2]); + +static BOOL Initialized; + +static u32 EnabledBits; +static u32 ResettingBits; +static s32 ResettingChan = 32; +static u32 RecalibrateBits; +static u32 WaitingBits; +static u32 CheckingBits; +static u32 PendingBits; + +static u32 XPatchBits = PAD_CHAN0_BIT | PAD_CHAN1_BIT | PAD_CHAN2_BIT | PAD_CHAN3_BIT; + +static u32 AnalogMode = 0x00000300u; + +u32 __PADSpec; +static u32 Spec = 5; +static void (*MakeStatus)(s32, PADStatus*, u32[2]) = SPEC2_MakeStatus; + +static u32 Type[SI_MAX_CHAN]; +static PADStatus Origin[SI_MAX_CHAN]; + +static u32 CmdReadOrigin = 0x41 << 24; +static u32 CmdCalibrate = 0x42 << 24; +static u32 CmdProbeDevice[SI_MAX_CHAN]; + +static BOOL OnReset(BOOL final); + +static OSResetFunctionInfo ResetFunctionInfo = {OnReset, 127}; + +static void (*SamplingCallback)(void); + +static void PADEnable(s32 chan) { + u32 cmd; + u32 chanBit; + u32 data[2]; + + chanBit = PAD_CHAN0_BIT >> chan; + EnabledBits |= chanBit; + SIGetResponse(chan, data); + cmd = (0x40 << 16) | AnalogMode; + SISetCommand(chan, cmd); + SIEnablePolling(EnabledBits); +} + +static void PADDisable(s32 chan) { + BOOL enabled; + u32 chanBit; + + enabled = OSDisableInterrupts(); + + chanBit = PAD_CHAN0_BIT >> chan; + SIDisablePolling(chanBit); + EnabledBits &= ~chanBit; + WaitingBits &= ~chanBit; + CheckingBits &= ~chanBit; + PendingBits &= ~chanBit; + OSSetWirelessID(chan, 0); + + OSRestoreInterrupts(enabled); +} + +static void DoReset(void) { + u32 chanBit; + + ResettingChan = __cntlzw(ResettingBits); + if (ResettingChan == 32) { + return; + } + + chanBit = PAD_CHAN0_BIT >> ResettingChan; + ResettingBits &= ~chanBit; + + memset(&Origin[ResettingChan], 0, sizeof(PADStatus)); + SIGetTypeAsync(ResettingChan, PADTypeAndStatusCallback); +} + +static void UpdateOrigin(s32 chan) { + PADStatus* origin; + u32 chanBit = PAD_CHAN0_BIT >> chan; + + origin = &Origin[chan]; + switch (AnalogMode & 0x00000700u) { + case 0x00000000u: + case 0x00000500u: + case 0x00000600u: + case 0x00000700u: + origin->triggerL &= ~15; + origin->triggerR &= ~15; + origin->analogA &= ~15; + origin->analogB &= ~15; + break; + case 0x00000100u: + origin->substickX &= ~15; + origin->substickY &= ~15; + origin->analogA &= ~15; + origin->analogB &= ~15; + break; + case 0x00000200u: + origin->substickX &= ~15; + origin->substickY &= ~15; + origin->triggerL &= ~15; + origin->triggerR &= ~15; + break; + case 0x00000300u: + break; + case 0x00000400u: + break; + } + + origin->stickX -= 128; + origin->stickY -= 128; + origin->substickX -= 128; + origin->substickY -= 128; + + if (XPatchBits & chanBit) { + if (64 < origin->stickX && (SIGetType(chan) & 0xffff0000) == SI_GC_CONTROLLER) { + origin->stickX = 0; + } + } +} + +static void PADOriginCallback(s32 chan, u32 error, OSContext* context) { + if (!(error & + (SI_ERROR_UNDER_RUN | SI_ERROR_OVER_RUN | SI_ERROR_NO_RESPONSE | SI_ERROR_COLLISION))) { + UpdateOrigin(ResettingChan); + PADEnable(ResettingChan); + } + DoReset(); +} + +static void PADOriginUpdateCallback(s32 chan, u32 error, OSContext* context) { + + if (!(EnabledBits & (PAD_CHAN0_BIT >> chan))) { + return; + } + + if (!(error & + (SI_ERROR_UNDER_RUN | SI_ERROR_OVER_RUN | SI_ERROR_NO_RESPONSE | SI_ERROR_COLLISION))) { + UpdateOrigin(chan); + } + + if (error & SI_ERROR_NO_RESPONSE) { + PADDisable(chan); + } +} + +static void PADProbeCallback(s32 chan, u32 error, OSContext* context) { + if (!(error & + (SI_ERROR_UNDER_RUN | SI_ERROR_OVER_RUN | SI_ERROR_NO_RESPONSE | SI_ERROR_COLLISION))) { + PADEnable(ResettingChan); + WaitingBits |= PAD_CHAN0_BIT >> ResettingChan; + } + DoReset(); +} + +static void PADTypeAndStatusCallback(s32 chan, u32 type) { + u32 chanBit; + u32 recalibrate; + BOOL rc = TRUE; + u32 error; + chanBit = PAD_CHAN0_BIT >> ResettingChan; + error = type & 0xFF; + recalibrate = RecalibrateBits & chanBit; + RecalibrateBits &= ~chanBit; + + if (error & + (SI_ERROR_UNDER_RUN | SI_ERROR_OVER_RUN | SI_ERROR_NO_RESPONSE | SI_ERROR_COLLISION)) { + DoReset(); + return; + } + + type &= ~0xFF; + + Type[ResettingChan] = type; + + if ((type & SI_TYPE_MASK) != SI_TYPE_GC || !(type & SI_GC_STANDARD)) { + DoReset(); + return; + } + + if (Spec < PAD_SPEC_2) { + PADEnable(ResettingChan); + DoReset(); + return; + } + + if (!(type & SI_GC_WIRELESS) || (type & SI_WIRELESS_IR)) { + if (recalibrate) { + rc = SITransfer(ResettingChan, &CmdCalibrate, 3, &Origin[ResettingChan], 10, + PADOriginCallback, 0); + } else { + rc = SITransfer(ResettingChan, &CmdReadOrigin, 1, &Origin[ResettingChan], 10, + PADOriginCallback, 0); + } + } else if ((type & SI_WIRELESS_FIX_ID) && (type & SI_WIRELESS_CONT_MASK) == SI_WIRELESS_CONT && + !(type & SI_WIRELESS_LITE)) { + if (type & SI_WIRELESS_RECEIVED) { + rc = SITransfer(ResettingChan, &CmdReadOrigin, 1, &Origin[ResettingChan], 10, + PADOriginCallback, 0); + } else { + rc = SITransfer(ResettingChan, &CmdProbeDevice[ResettingChan], 3, &Origin[ResettingChan], 8, + PADProbeCallback, 0); + } + } + if (!rc) { + PendingBits |= chanBit; + DoReset(); + return; + } +} + +static void PADReceiveCheckCallback(s32 chan, u32 type) { + u32 error; + u32 chanBit; + + chanBit = PAD_CHAN0_BIT >> chan; + if (!(EnabledBits & chanBit)) { + return; + } + + error = type & 0xFF; + type &= ~0xFF; + + WaitingBits &= ~chanBit; + CheckingBits &= ~chanBit; + + if (!(error & + (SI_ERROR_UNDER_RUN | SI_ERROR_OVER_RUN | SI_ERROR_NO_RESPONSE | SI_ERROR_COLLISION)) && + (type & SI_GC_WIRELESS) && (type & SI_WIRELESS_FIX_ID) && (type & SI_WIRELESS_RECEIVED) && + !(type & SI_WIRELESS_IR) && (type & SI_WIRELESS_CONT_MASK) == SI_WIRELESS_CONT && + !(type & SI_WIRELESS_LITE)) { + SITransfer(chan, &CmdReadOrigin, 1, &Origin[chan], 10, PADOriginUpdateCallback, 0); + } else { + PADDisable(chan); + } +} + +BOOL PADReset(u32 mask) { + BOOL enabled; + u32 diableBits; + + enabled = OSDisableInterrupts(); + + mask |= PendingBits; + PendingBits = 0; + mask &= ~(WaitingBits | CheckingBits); + ResettingBits |= mask; + diableBits = ResettingBits & EnabledBits; + EnabledBits &= ~mask; + + if (Spec == PAD_SPEC_4) { + RecalibrateBits |= mask; + } + + SIDisablePolling(diableBits); + + if (ResettingChan == 32) { + DoReset(); + } + OSRestoreInterrupts(enabled); + return TRUE; +} + +BOOL PADRecalibrate(u32 mask) { + BOOL enabled; + u32 disableBits; + + enabled = OSDisableInterrupts(); + + mask |= PendingBits; + PendingBits = 0; + mask &= ~(WaitingBits | CheckingBits); + ResettingBits |= mask; + disableBits = ResettingBits & EnabledBits; + EnabledBits &= ~mask; + + if (!(UnkVal & 0x40)) { + RecalibrateBits |= mask; + } + + SIDisablePolling(disableBits); + if (ResettingChan == 32) { + DoReset(); + } + OSRestoreInterrupts(enabled); + return TRUE; +} + +BOOL PADInit() { + s32 chan; + if (Initialized) { + return TRUE; + } + + OSRegisterVersion(__PADVersion); + + if (__PADSpec) { + PADSetSpec(__PADSpec); + } + + Initialized = TRUE; + + if (__PADFixBits != 0) { + OSTime time = OSGetTime(); + __OSWirelessPadFixMode = (u16)((((time)&0xffff) + ((time >> 16) & 0xffff) + + ((time >> 32) & 0xffff) + ((time >> 48) & 0xffff)) & + 0x3fffu); + RecalibrateBits = PAD_CHAN0_BIT | PAD_CHAN1_BIT | PAD_CHAN2_BIT | PAD_CHAN3_BIT; + } + + for (chan = 0; chan < SI_MAX_CHAN; ++chan) { + CmdProbeDevice[chan] = (0x4D << 24) | (chan << 22) | ((__OSWirelessPadFixMode & 0x3fffu) << 8); + } + + SIRefreshSamplingRate(); + OSRegisterResetFunction(&ResetFunctionInfo); + + return PADReset(PAD_CHAN0_BIT | PAD_CHAN1_BIT | PAD_CHAN2_BIT | PAD_CHAN3_BIT); +} + +#define offsetof(type, memb) ((u32) & ((type*)0)->memb) + +u32 PADRead(PADStatus* status) { + BOOL enabled; + s32 chan; + u32 data[2]; + u32 chanBit; + u32 sr; + int chanShift; + u32 motor; + + enabled = OSDisableInterrupts(); + + motor = 0; + for (chan = 0; chan < SI_MAX_CHAN; chan++, status++) { + chanBit = PAD_CHAN0_BIT >> chan; + chanShift = 8 * (SI_MAX_CHAN - 1 - chan); + + if (PendingBits & chanBit) { + PADReset(0); + status->err = PAD_ERR_NOT_READY; + memset(status, 0, offsetof(PADStatus, err)); + continue; + } + + if ((ResettingBits & chanBit) || ResettingChan == chan) { + status->err = PAD_ERR_NOT_READY; + memset(status, 0, offsetof(PADStatus, err)); + continue; + } + + if (!(EnabledBits & chanBit)) { + status->err = (s8)PAD_ERR_NO_CONTROLLER; + memset(status, 0, offsetof(PADStatus, err)); + continue; + } + + if (SIIsChanBusy(chan)) { + status->err = PAD_ERR_TRANSFER; + memset(status, 0, offsetof(PADStatus, err)); + continue; + } + + sr = SIGetStatus(chan); + if (sr & SI_ERROR_NO_RESPONSE) { + SIGetResponse(chan, data); + + if (WaitingBits & chanBit) { + status->err = (s8)PAD_ERR_NONE; + memset(status, 0, offsetof(PADStatus, err)); + + if (!(CheckingBits & chanBit)) { + CheckingBits |= chanBit; + SIGetTypeAsync(chan, PADReceiveCheckCallback); + } + continue; + } + + PADDisable(chan); + + status->err = (s8)PAD_ERR_NO_CONTROLLER; + memset(status, 0, offsetof(PADStatus, err)); + continue; + } + + if (!(SIGetType(chan) & SI_GC_NOMOTOR)) { + motor |= chanBit; + } + + if (!SIGetResponse(chan, data)) { + status->err = PAD_ERR_TRANSFER; + memset(status, 0, offsetof(PADStatus, err)); + continue; + } + + if (data[0] & 0x80000000) { + status->err = PAD_ERR_TRANSFER; + memset(status, 0, offsetof(PADStatus, err)); + continue; + } + + MakeStatus(chan, status, data); + + // Check and clear PAD_ORIGIN bit + if (status->button & 0x2000) { + status->err = PAD_ERR_TRANSFER; + memset(status, 0, offsetof(PADStatus, err)); + + // Get origin. It is okay if the following transfer fails + // since the PAD_ORIGIN bit remains until the read origin + // command complete. + SITransfer(chan, &CmdReadOrigin, 1, &Origin[chan], 10, PADOriginUpdateCallback, 0); + continue; + } + + status->err = PAD_ERR_NONE; + + // Clear PAD_INTERFERE bit + status->button &= ~0x0080; + } + + OSRestoreInterrupts(enabled); + return motor; +} + +void PADControlAllMotors(const u32* commandArray) { + BOOL enabled; + int chan; + u32 command; + BOOL commit; + u32 chanBit; + + enabled = OSDisableInterrupts(); + commit = FALSE; + for (chan = 0; chan < SI_MAX_CHAN; chan++, commandArray++) { + chanBit = PAD_CHAN0_BIT >> chan; + if ((EnabledBits & chanBit) && !(SIGetType(chan) & SI_GC_NOMOTOR)) { + command = *commandArray; + if (Spec < PAD_SPEC_2 && command == PAD_MOTOR_STOP_HARD) { + command = PAD_MOTOR_STOP; + } + + SISetCommand(chan, (0x40 << 16) | AnalogMode | (command & (0x00000001 | 0x00000002))); + commit = TRUE; + } + } + if (commit) { + SITransferCommands(); + } + OSRestoreInterrupts(enabled); +} + +void PADControlMotor(s32 chan, u32 command) { + BOOL enabled; + u32 chanBit; + + enabled = OSDisableInterrupts(); + chanBit = PAD_CHAN0_BIT >> chan; + if ((EnabledBits & chanBit) && !(SIGetType(chan) & SI_GC_NOMOTOR)) { + if (Spec < PAD_SPEC_2 && command == PAD_MOTOR_STOP_HARD) { + command = PAD_MOTOR_STOP; + } + + SISetCommand(chan, (0x40 << 16) | AnalogMode | (command & (0x00000001 | 0x00000002))); + SITransferCommands(); + } + OSRestoreInterrupts(enabled); +} + +void PADSetSpec(u32 spec) { + __PADSpec = 0; + switch (spec) { + case PAD_SPEC_0: + MakeStatus = SPEC0_MakeStatus; + break; + case PAD_SPEC_1: + MakeStatus = SPEC1_MakeStatus; + break; + case PAD_SPEC_2: + case PAD_SPEC_3: + case PAD_SPEC_4: + case PAD_SPEC_5: + MakeStatus = SPEC2_MakeStatus; + break; + } + Spec = spec; +} + +u32 PADGetSpec(void) { return Spec; } + +static void SPEC0_MakeStatus(s32 chan, PADStatus* status, u32 data[2]) { + status->button = 0; + status->button |= ((data[0] >> 16) & 0x0008) ? PAD_BUTTON_A : 0; + status->button |= ((data[0] >> 16) & 0x0020) ? PAD_BUTTON_B : 0; + status->button |= ((data[0] >> 16) & 0x0100) ? PAD_BUTTON_X : 0; + status->button |= ((data[0] >> 16) & 0x0001) ? PAD_BUTTON_Y : 0; + status->button |= ((data[0] >> 16) & 0x0010) ? PAD_BUTTON_START : 0; + status->stickX = (s8)(data[1] >> 16); + status->stickY = (s8)(data[1] >> 24); + status->substickX = (s8)(data[1]); + status->substickY = (s8)(data[1] >> 8); + status->triggerL = (u8)(data[0] >> 8); + status->triggerR = (u8)data[0]; + status->analogA = 0; + status->analogB = 0; + if (170 <= status->triggerL) { + status->button |= PAD_TRIGGER_L; + } + if (170 <= status->triggerR) { + status->button |= PAD_TRIGGER_R; + } + status->stickX -= 128; + status->stickY -= 128; + status->substickX -= 128; + status->substickY -= 128; +} + +static void SPEC1_MakeStatus(s32 chan, PADStatus* status, u32 data[2]) { + + status->button = 0; + status->button |= ((data[0] >> 16) & 0x0080) ? PAD_BUTTON_A : 0; + status->button |= ((data[0] >> 16) & 0x0100) ? PAD_BUTTON_B : 0; + status->button |= ((data[0] >> 16) & 0x0020) ? PAD_BUTTON_X : 0; + status->button |= ((data[0] >> 16) & 0x0010) ? PAD_BUTTON_Y : 0; + status->button |= ((data[0] >> 16) & 0x0200) ? PAD_BUTTON_START : 0; + + status->stickX = (s8)(data[1] >> 16); + status->stickY = (s8)(data[1] >> 24); + status->substickX = (s8)(data[1]); + status->substickY = (s8)(data[1] >> 8); + + status->triggerL = (u8)(data[0] >> 8); + status->triggerR = (u8)data[0]; + + status->analogA = 0; + status->analogB = 0; + + if (170 <= status->triggerL) { + status->button |= PAD_TRIGGER_L; + } + if (170 <= status->triggerR) { + status->button |= PAD_TRIGGER_R; + } + + status->stickX -= 128; + status->stickY -= 128; + status->substickX -= 128; + status->substickY -= 128; +} + +static s8 ClampS8(s8 var, s8 org) { + if (0 < org) { + s8 min = (s8)(-128 + org); + if (var < min) { + var = min; + } + } else if (org < 0) { + s8 max = (s8)(127 + org); + if (max < var) { + var = max; + } + } + return var -= org; +} + +static u8 ClampU8(u8 var, u8 org) { + if (var < org) { + var = org; + } + return var -= org; +} + +#define PAD_ALL \ + (PAD_BUTTON_LEFT | PAD_BUTTON_RIGHT | PAD_BUTTON_DOWN | PAD_BUTTON_UP | PAD_TRIGGER_Z | \ + PAD_TRIGGER_R | PAD_TRIGGER_L | PAD_BUTTON_A | PAD_BUTTON_B | PAD_BUTTON_X | PAD_BUTTON_Y | \ + PAD_BUTTON_MENU | 0x2000 | 0x0080) + +static void SPEC2_MakeStatus(s32 chan, PADStatus* status, u32 data[2]) { + PADStatus* origin; + + status->button = (u16)((data[0] >> 16) & PAD_ALL); + status->stickX = (s8)(data[0] >> 8); + status->stickY = (s8)(data[0]); + + switch (AnalogMode & 0x00000700) { + case 0x00000000: + case 0x00000500: + case 0x00000600: + case 0x00000700: + status->substickX = (s8)(data[1] >> 24); + status->substickY = (s8)(data[1] >> 16); + status->triggerL = (u8)(((data[1] >> 12) & 0x0f) << 4); + status->triggerR = (u8)(((data[1] >> 8) & 0x0f) << 4); + status->analogA = (u8)(((data[1] >> 4) & 0x0f) << 4); + status->analogB = (u8)(((data[1] >> 0) & 0x0f) << 4); + break; + case 0x00000100: + status->substickX = (s8)(((data[1] >> 28) & 0x0f) << 4); + status->substickY = (s8)(((data[1] >> 24) & 0x0f) << 4); + status->triggerL = (u8)(data[1] >> 16); + status->triggerR = (u8)(data[1] >> 8); + status->analogA = (u8)(((data[1] >> 4) & 0x0f) << 4); + status->analogB = (u8)(((data[1] >> 0) & 0x0f) << 4); + break; + case 0x00000200: + status->substickX = (s8)(((data[1] >> 28) & 0x0f) << 4); + status->substickY = (s8)(((data[1] >> 24) & 0x0f) << 4); + status->triggerL = (u8)(((data[1] >> 20) & 0x0f) << 4); + status->triggerR = (u8)(((data[1] >> 16) & 0x0f) << 4); + status->analogA = (u8)(data[1] >> 8); + status->analogB = (u8)(data[1] >> 0); + break; + case 0x00000300: + status->substickX = (s8)(data[1] >> 24); + status->substickY = (s8)(data[1] >> 16); + status->triggerL = (u8)(data[1] >> 8); + status->triggerR = (u8)(data[1] >> 0); + status->analogA = 0; + status->analogB = 0; + break; + case 0x00000400: + status->substickX = (s8)(data[1] >> 24); + status->substickY = (s8)(data[1] >> 16); + status->triggerL = 0; + status->triggerR = 0; + status->analogA = (u8)(data[1] >> 8); + status->analogB = (u8)(data[1] >> 0); + break; + } + + status->stickX -= 128; + status->stickY -= 128; + status->substickX -= 128; + status->substickY -= 128; + + origin = &Origin[chan]; + status->stickX = ClampS8(status->stickX, origin->stickX); + status->stickY = ClampS8(status->stickY, origin->stickY); + status->substickX = ClampS8(status->substickX, origin->substickX); + status->substickY = ClampS8(status->substickY, origin->substickY); + status->triggerL = ClampU8(status->triggerL, origin->triggerL); + status->triggerR = ClampU8(status->triggerR, origin->triggerR); +} + +BOOL PADGetType(s32 chan, u32* type) { + u32 chanBit; + + *type = SIGetType(chan); + chanBit = PAD_CHAN0_BIT >> chan; + if ((ResettingBits & chanBit) || ResettingChan == chan || !(EnabledBits & chanBit)) { + return FALSE; + } + return TRUE; +} + +BOOL PADSync(void) { return ResettingBits == 0 && ResettingChan == 32 && !SIBusy(); } + +void PADSetAnalogMode(u32 mode) { + BOOL enabled; + u32 mask; + + enabled = OSDisableInterrupts(); + AnalogMode = mode << 8; + mask = EnabledBits; + + EnabledBits &= ~mask; + WaitingBits &= ~mask; + CheckingBits &= ~mask; + + SIDisablePolling(mask); + OSRestoreInterrupts(enabled); +} + +static BOOL OnReset(BOOL f) { + static BOOL recalibrated = FALSE; + BOOL sync; + + if (SamplingCallback) { + PADSetSamplingCallback(NULL); + } + + if (!f) { + sync = PADSync(); + if (!recalibrated && sync) { + recalibrated = PADRecalibrate(PAD_CHAN0_BIT | PAD_CHAN1_BIT | PAD_CHAN2_BIT | PAD_CHAN3_BIT); + return FALSE; + } + return sync; + } else { + recalibrated = FALSE; + } + + return TRUE; +} + +void __PADDisableXPatch(void) { XPatchBits = 0; } + +static void SamplingHandler(__OSInterrupt interrupt, OSContext* context) { + OSContext exceptionContext; + + if (SamplingCallback) { + OSClearContext(&exceptionContext); + OSSetCurrentContext(&exceptionContext); + SamplingCallback(); + OSClearContext(&exceptionContext); + OSSetCurrentContext(context); + } +} + +#ifdef FULL_FRANK +PADSamplingCallback PADSetSamplingCallback(PADSamplingCallback callback) { + PADSamplingCallback prev; + + prev = SamplingCallback; + SamplingCallback = callback; + if (callback) { + SIRegisterPollingHandler(SamplingHandler); + } else { + SIUnregisterPollingHandler(SamplingHandler); + } + return prev; +} +#else +/* clang-format off */ +#pragma push +#pragma optimization_level 0 +#pragma optimizewithasm off +asm PADSamplingCallback PADSetSamplingCallback(PADSamplingCallback callback) { + nofralloc + mflr r0 + cmplwi r3, 0 + stw r0, 4(r1) + stwu r1, -0x18(r1) + stw r31, 0x14(r1) + lwz r31, SamplingCallback + stw r3, SamplingCallback + beq lbl_803875E4 + lis r3, SamplingHandler@ha + addi r3, r3, SamplingHandler@l + bl SIRegisterPollingHandler + b lbl_803875F0 +lbl_803875E4: + lis r3, SamplingHandler@ha + addi r3, r3, SamplingHandler@l + bl SIUnregisterPollingHandler +lbl_803875F0: + mr r3, r31 + lwz r0, 0x1c(r1) + lwz r31, 0x14(r1) + addi r1, r1, 0x18 + mtlr r0 + blr +} +#pragma pop +#endif + +#ifdef FULL_FRANK +BOOL __PADDisableRecalibration(BOOL disable) { + BOOL enabled; + BOOL prev; + + enabled = OSDisableInterrupts(); + prev = (UnkVal & 0x40) ? TRUE : FALSE; + UnkVal &= (u8)~0x40; + if (disable) { + UnkVal |= 0x40; + } + OSRestoreInterrupts(enabled); + return prev; +} +#else +/* clang-format off */ +#pragma push +#pragma optimization_level 0 +#pragma optimizewithasm off +asm BOOL __PADDisableRecalibration(BOOL disable) { + nofralloc + mflr r0 + stw r0, 4(r1) + stwu r1, -0x18(r1) + stw r31, 0x14(r1) + stw r30, 0x10(r1) + mr r30, r3 + bl OSDisableInterrupts + lis r4, UnkVal@ha + lbz r0, UnkVal@l(r4) + rlwinm. r0, r0, 0, 0x19, 0x19 + beq lbl_8038763C + li r31, 1 + b lbl_80387640 +lbl_8038763C: + li r31, 0 +lbl_80387640: + lis r4, UnkVal@ha + lbz r0, UnkVal@l(r4) + andi. r0, r0, 0xbf + cmpwi r30, 0 + stb r0, UnkVal@l(r4) + beq lbl_80387664 + lbz r0, UnkVal@l(r4) + ori r0, r0, 0x40 + stb r0, UnkVal@l(r4) +lbl_80387664: + bl OSRestoreInterrupts + mr r3, r31 + lwz r0, 0x1c(r1) + lwz r31, 0x14(r1) + lwz r30, 0x10(r1) + addi r1, r1, 0x18 + mtlr r0 + blr +} +/* clang-format on */ +#pragma pop +#endif -- cgit v1.2.3-13-gbd6f