blob: 98d38991b6f1b7654b4f3f627f78c2645104571e (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
|
#include <dolphin/dolphin.h>
#include <dolphin/si.h>
static void (*SamplingCallback)();
u32 __SISteeringEnableBits;
void __SISteeringEnable(s32 chan) {
u32 cmd;
u32 chanBit;
u32 data[2];
chanBit = 0x80000000 >> chan;
if (!(__SISteeringEnableBits & chanBit)) {
__SISteeringEnableBits |= chanBit;
SIGetResponse(chan, &data);
cmd = 0x300680;
SISetCommand(chan, cmd);
SIEnablePolling(__SISteeringEnableBits);
}
}
void __SISteeringDisable(s32 chan) {
u32 chanBit;
chanBit = 0x80000000 >> chan;
SIDisablePolling(chanBit);
__SISteeringEnableBits &= ~chanBit;
}
s32 SIReadSteering(s32 chan, SISteeringStatus* status) {
BOOL enabled;
SISteeringControl* sc;
u32 data[2];
u32 chanBit;
u32 sr;
s32 ret;
enabled = OSDisableInterrupts();
sc = &__SISteering[chan];
chanBit = 0x80000000 >> chan;
if (SIIsChanBusy(chan)) {
sc->ret = -2;
} else if (!(__SISteeringEnableBits & chanBit)) {
sc->ret = -1;
} else {
sr = SIGetStatus(chan);
if (sr & 8) {
SIGetResponse(chan, &data);
__SISteeringDisable(chan);
sc->ret = -1;
} else if ((SIGetResponse(chan, &data) == 0) || (data[0] & 0x80000000)) {
sc->ret = -3;
} else {
sc->ret = 0;
if (status != 0) {
status->button = data[0] >> 0x10;
status->misc = data[0] >> 8;
status->steering = (data[0] & 0xFF) - 0x80;
status->gas = data[1] >> 0x18;
status->brake = data[1] >> 0x10;
status->left = data[1] >> 8;
status->right = data[1];
}
}
}
if (status != 0) {
status->err = sc->ret;
}
ret = sc->ret;
OSRestoreInterrupts(enabled);
return ret;
}
static void SamplingHandler(__OSInterrupt interrupt, OSContext* context) {
OSContext exceptionContext;
if (SamplingCallback != 0) {
OSClearContext(&exceptionContext);
OSSetCurrentContext(&exceptionContext);
SamplingCallback();
OSClearContext(&exceptionContext);
OSSetCurrentContext(context);
}
}
void (* SISetSteeringSamplingCallback(void (*callback)()))() {
void (*prev)() = SamplingCallback;
SamplingCallback = callback;
if (callback != 0) {
SIRegisterPollingHandler(SamplingHandler);
} else {
SIUnregisterPollingHandler(SamplingHandler);
}
return prev;
}
void SIControlSteering(s32 chan, u32 control, s32 level) {
BOOL enabled;
u32 chanBit;
u32 command;
control &= 0x600;
if (level <= -0x80) {
command = 0;
} else if (level >= 0x80) {
command = 0x100;
} else {
command = level + 0x80;
}
command |= control;
command &= 0x7FF;
enabled = OSDisableInterrupts();
chanBit = 0x80000000 >> chan;
if (__SISteeringEnableBits & chanBit) {
command |= 0x300000;
SISetCommand(chan, command);
SITransferCommands();
}
OSRestoreInterrupts(enabled);
}
|
