kobayashi90/marioparty4
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Matched pad.c and a few mtx files

Browse source
This commit is contained in:
dbalatoni13 2024-11-12 00:08:39 +01:00
parent 6dfc6da0e4
commit 4bbaec7455
12 changed files with 3216 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

4
config/GMPE01_00/rels/m420dll/symbols.txt
View file

@ -207,9 +207,9 @@ lbl_1_data_0 = .data:0x00000000; // type:object size:0x1E scope:local data:strin
lbl_1_data_1E = .data:0x0000001E; // type:object size:0x1E scope:local data:string
lbl_1_data_3C = .data:0x0000003C; // type:object size:0x11 scope:local data:string
lbl_1_data_50 = .data:0x00000050; // type:object size:0x80
lbl_1_data_D0 = .data:0x000000D0; // type:object size:0x1B data:string
lbl_1_data_D0 = .data:0x000000D0; // type:object size:0x1B scope:local data:string
lbl_1_data_EC = .data:0x000000EC; // type:object size:0x30
lbl_1_data_11C = .data:0x0000011C; // type:object size:0x19 data:string
lbl_1_data_11C = .data:0x0000011C; // type:object size:0x19 scope:local data:string
lbl_1_data_135 = .data:0x00000135; // type:object size:0x1
lbl_1_data_138 = .data:0x00000138; // type:object size:0x1C
lbl_1_data_154 = .data:0x00000154; // type:object size:0x10 data:4byte

7
config/GMPE01_00/splits.txt
View file

@ -680,12 +680,12 @@ dolphin/mtx/vec.c:
.sdata2 start:0x801D6380 end:0x801D6390
dolphin/mtx/quat.c:
.text start:0x800BC2C4 end:0x800BC710
.text start:0x800BC2C4 end:0x800BC884
.rodata start:0x8011E430 end:0x8011E440
.sdata2 start:0x801D6390 end:0x801D63B8
dolphin/mtx/psmtx.c:
.text start:0x800BC710 end:0x800BC9E8
.text start:0x800BC884 end:0x800BC9E8
dolphin/dvd/dvdlow.c:
.text start:0x800BC9E8 end:0x800BD83C
@ -742,7 +742,7 @@ dolphin/demo/DEMOPuts.c:
dolphin/demo/DEMOStats.c:
.text start:0x800C2F84 end:0x800C3CDC
.data start:0x8013D5E0 end:0x8013D748
.data start:0x8013D5E0 end:0x8013D738
.sdata start:0x801D3908 end:0x801D3910
.sbss start:0x801D4488 end:0x801D44E8
.sdata2 start:0x801D63D8 end:0x801D63E8
@ -753,6 +753,7 @@ dolphin/pad/Padclamp.c:
dolphin/pad/Pad.c:
.text start:0x800C3F14 end:0x800C59DC
.data start:0x8013D738 end:0x8013D748
.bss start:0x801A61B0 end:0x801A6200
.sdata start:0x801D3918 end:0x801D3938
.sbss start:0x801D44E8 end:0x801D4510

44
config/GMPE01_00/symbols.txt
View file

@ -4345,10 +4345,10 @@ lbl_8013C2F0 = .data:0x8013C2F0; // type:object size:0x78
YearDays = .data:0x8013C368; // type:object size:0x30 scope:local
LeapYearDays = .data:0x8013C398; // type:object size:0x30 scope:local
lbl_8013C3C8 = .data:0x8013C3C8; // type:object size:0x18 scope:local data:string
lbl_8013C3E0 = .data:0x8013C3E0; // type:object size:0xC8 data:string
lbl_8013C4A8 = .data:0x8013C4A8; // type:object size:0x38
lbl_8013C4E0 = .data:0x8013C4E0; // type:object size:0x34 data:string
lbl_8013C514 = .data:0x8013C514; // type:object size:0x34
lbl_8013C3E0 = .data:0x8013C3E0; // type:object size:0xC8 scope:local data:string
lbl_8013C4A8 = .data:0x8013C4A8; // type:object size:0x38 scope:local
lbl_8013C4E0 = .data:0x8013C4E0; // type:object size:0x34 scope:local data:string
lbl_8013C514 = .data:0x8013C514; // type:object size:0x34 scope:local
@13 = .data:0x8013C548; // type:object size:0x15 scope:local data:string
@293 = .data:0x8013C594; // type:object size:0x20 scope:local
jumptable_8013C5B4 = .data:0x8013C5B4; // type:object size:0x40 scope:local
@ -4373,7 +4373,7 @@ jumptable_8013D698 = .data:0x8013D698; // type:object size:0x28 scope:local
jumptable_8013D6C0 = .data:0x8013D6C0; // type:object size:0x28 scope:local
jumptable_8013D6E8 = .data:0x8013D6E8; // type:object size:0x28 scope:local
jumptable_8013D710 = .data:0x8013D710; // type:object size:0x28 scope:local
lbl_8013D738 = .data:0x8013D738; // type:object size:0x10
ResetFunctionInfo = .data:0x8013D738; // type:object size:0x10
lbl_8013D748 = .data:0x8013D748; // type:object size:0x38
lbl_8013D780 = .data:0x8013D780; // type:object size:0x140
jumptable_8013D8C0 = .data:0x8013D8C0; // type:object size:0x68 scope:local
@ -4976,8 +4976,8 @@ __OSArenaLo = .sdata:0x801D38C0; // type:object size:0x4 scope:local data:4byte
@32 = .sdata:0x801D38C8; // type:object size:0x2 scope:local data:string
fontEncode$80 = .sdata:0x801D38D0; // type:object size:0x8 scope:local data:2byte
Unit01 = .sdata:0x801D38D8; // type:object size:0x8
FirstRead = .sdata:0x801D38E0; // type:object size:0x8 scope:local data:4byte
lbl_801D38E8 = .sdata:0x801D38E8; // type:object size:0x8 data:string
FirstRead = .sdata:0x801D38E0; // type:object size:0x4 scope:local data:4byte
lbl_801D38E8 = .sdata:0x801D38E8; // type:object size:0x8 scope:local data:string
autoInvalidation = .sdata:0x801D38F0; // type:object size:0x4 scope:local data:4byte
@35 = .sdata:0x801D38F8; // type:object size:0x2 scope:local data:string
@40 = .sdata:0x801D38FC; // type:object size:0x4 scope:local data:string
@ -7284,21 +7284,21 @@ lbl_801D6338 = .sdata2:0x801D6338; // type:object size:0x4 data:float
lbl_801D633C = .sdata2:0x801D633C; // type:object size:0x4 data:float
lbl_801D6340 = .sdata2:0x801D6340; // type:object size:0x8 data:double
lbl_801D6348 = .sdata2:0x801D6348; // type:object size:0x8 data:float
lbl_801D6350 = .sdata2:0x801D6350; // type:object size:0x4 data:float
lbl_801D6354 = .sdata2:0x801D6354; // type:object size:0x4 data:float
lbl_801D6358 = .sdata2:0x801D6358; // type:object size:0x4 data:float
lbl_801D635C = .sdata2:0x801D635C; // type:object size:0x4 data:float
lbl_801D6360 = .sdata2:0x801D6360; // type:object size:0x8 data:float
lbl_801D6368 = .sdata2:0x801D6368; // type:object size:0x4 data:float
lbl_801D636C = .sdata2:0x801D636C; // type:object size:0x4 data:float
lbl_801D6370 = .sdata2:0x801D6370; // type:object size:0x4 data:float
lbl_801D6374 = .sdata2:0x801D6374; // type:object size:0x4 data:float
lbl_801D6378 = .sdata2:0x801D6378; // type:object size:0x4 data:float
lbl_801D637C = .sdata2:0x801D637C; // type:object size:0x4 data:float
lbl_801D6380 = .sdata2:0x801D6380; // type:object size:0x4 data:float
lbl_801D6384 = .sdata2:0x801D6384; // type:object size:0x4 data:float
lbl_801D6388 = .sdata2:0x801D6388; // type:object size:0x4 data:float
lbl_801D638C = .sdata2:0x801D638C; // type:object size:0x4 data:float
lbl_801D6350 = .sdata2:0x801D6350; // type:object size:0x4 scope:local data:float
lbl_801D6354 = .sdata2:0x801D6354; // type:object size:0x4 scope:local data:float
lbl_801D6358 = .sdata2:0x801D6358; // type:object size:0x4 scope:local data:float
lbl_801D635C = .sdata2:0x801D635C; // type:object size:0x4 scope:local data:float
lbl_801D6360 = .sdata2:0x801D6360; // type:object size:0x8 scope:local data:float
lbl_801D6368 = .sdata2:0x801D6368; // type:object size:0x4 scope:local data:float
lbl_801D636C = .sdata2:0x801D636C; // type:object size:0x4 scope:local data:float
lbl_801D6370 = .sdata2:0x801D6370; // type:object size:0x4 scope:local data:float
lbl_801D6374 = .sdata2:0x801D6374; // type:object size:0x4 scope:local data:float
lbl_801D6378 = .sdata2:0x801D6378; // type:object size:0x4 scope:local data:float
lbl_801D637C = .sdata2:0x801D637C; // type:object size:0x4 scope:local data:float
lbl_801D6380 = .sdata2:0x801D6380; // type:object size:0x4 scope:local data:float
lbl_801D6384 = .sdata2:0x801D6384; // type:object size:0x4 scope:local data:float
lbl_801D6388 = .sdata2:0x801D6388; // type:object size:0x4 scope:local data:float
lbl_801D638C = .sdata2:0x801D638C; // type:object size:0x4 scope:local data:float
lbl_801D6390 = .sdata2:0x801D6390; // type:object size:0x4 data:float
lbl_801D6394 = .sdata2:0x801D6394; // type:object size:0x4 data:float
lbl_801D6398 = .sdata2:0x801D6398; // type:object size:0x8 data:double

8
configure.py
View file

@ -484,11 +484,11 @@ config.libs = [
"mtx",
[
Object(NonMatching, "dolphin/mtx/mtx.c"),
Object(NonMatching, "dolphin/mtx/mtxvec.c"),
Object(NonMatching, "dolphin/mtx/mtx44.c"),
Object(Matching, "dolphin/mtx/mtxvec.c"),
Object(Matching, "dolphin/mtx/mtx44.c"),
Object(NonMatching, "dolphin/mtx/vec.c"),
Object(NonMatching, "dolphin/mtx/quat.c"),
Object(NonMatching, "dolphin/mtx/psmtx.c"),
Object(Matching, "dolphin/mtx/psmtx.c"),
],
),
DolphinLib(
@ -521,7 +521,7 @@ config.libs = [
"pad",
[
Object(NonMatching, "dolphin/pad/Padclamp.c"),
Object(NonMatching, "dolphin/pad/Pad.c"),
Object(Matching, "dolphin/pad/Pad.c"),
],
),
DolphinLib(

1315
src/dolphin/mtx/mtx.c Normal file
View file

File diff suppressed because it is too large Load diff

99
src/dolphin/mtx/mtx44.c Normal file
View file

@ -0,0 +1,99 @@
#include "dolphin/mtx.h"
#include "math.h"
void C_MTXFrustum(Mtx44 m, f32 arg1, f32 arg2, f32 arg3, f32 arg4, f32 arg5, f32 arg6)
{
f32 tmp = 1.0f / (arg4 - arg3);
m[0][0] = (2 * arg5) * tmp;
m[0][1] = 0.0f;
m[0][2] = (arg4 + arg3) * tmp;
m[0][3] = 0.0f;
tmp = 1.0f / (arg1 - arg2);
m[1][0] = 0.0f;
m[1][1] = (2 * arg5) * tmp;
m[1][2] = (arg1 + arg2) * tmp;
m[1][3] = 0.0f;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
tmp = 1.0f / (arg6 - arg5);
m[2][2] = -(arg5)*tmp;
m[2][3] = -(arg6 * arg5) * tmp;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = -1.0f;
m[3][3] = 0.0f;
}
// Functions match but has issues with float constants
void C_MTXPerspective(Mtx44 m, f32 fovY, f32 aspect, f32 n, f32 f)
{
f32 angle = fovY * 0.5f;
f32 cot;
f32 tmp;
angle = MTXDegToRad(angle);
cot = 1.0f / tanf(angle);
m[0][0] = cot / aspect;
m[0][1] = 0.0f;
m[0][2] = 0.0f;
m[0][3] = 0.0f;
m[1][0] = 0.0f;
m[1][1] = cot;
m[1][2] = 0.0f;
m[1][3] = 0.0f;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
tmp = 1.0f / (f - n);
m[2][2] = -(n)*tmp;
m[2][3] = -(f * n) * tmp;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = -1.0f;
m[3][3] = 0.0f;
}
void C_MTXOrtho(Mtx44 m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 f)
{
f32 tmp = 1.0f / (r - l);
m[0][0] = 2.0f * tmp;
m[0][1] = 0.0f;
m[0][2] = 0.0f;
m[0][3] = -(r + l) * tmp;
tmp = 1.0f / (t - b);
m[1][0] = 0.0f;
m[1][1] = 2.0f * tmp;
m[1][2] = 0.0f;
m[1][3] = -(t + b) * tmp;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
tmp = 1.0f / (f - n);
m[2][2] = -(1.0f) * tmp;
m[2][3] = -(f)*tmp;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = 0.0f;
m[3][3] = 1.0f;
}
asm void PSMTX44Copy(register Mtx44 src, register Mtx44 dest)
{
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l fp1, 0(src), 0, 0;
psq_st fp1, 0(dest), 0, 0;
psq_l fp1, 8(src), 0, 0;
psq_st fp1, 8(dest), 0, 0;
psq_l fp1, 0x10(src), 0, 0;
psq_st fp1, 0x10(dest), 0, 0;
psq_l fp1, 0x18(src), 0, 0;
psq_st fp1, 0x18(dest), 0, 0;
psq_l fp1, 0x20(src), 0, 0;
psq_st fp1, 0x20(dest), 0, 0;
psq_l fp1, 0x28(src), 0, 0;
psq_st fp1, 0x28(dest), 0, 0;
psq_l fp1, 0x30(src), 0, 0;
psq_st fp1, 0x30(dest), 0, 0;
psq_l fp1, 0x38(src), 0, 0;
psq_st fp1, 0x38(dest), 0, 0;
blr;
#endif // clang-format on
}

146
src/dolphin/mtx/mtxvec.c Normal file
View file

@ -0,0 +1,146 @@
#include "dolphin/mtx.h"
asm void PSMTXMultVec(const register Mtx m, const register Vec* in, register Vec* out) {
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l fp0, 0(in), 0, 0;
psq_l fp2, 0(m), 0, 0;
psq_l fp1, 8(in), 1, 0;
ps_mul fp4, fp2, fp0;
psq_l fp3, 8(m), 0, 0;
ps_madd fp5, fp3, fp1, fp4;
psq_l fp8, 16(m), 0, 0;
ps_sum0 fp6, fp5, fp6, fp5;
psq_l fp9, 24(m), 0, 0;
ps_mul fp10, fp8, fp0;
psq_st fp6, 0(out), 1, 0;
ps_madd fp11, fp9, fp1, fp10;
psq_l fp2, 32(m), 0, 0;
ps_sum0 fp12, fp11, fp12, fp11;
psq_l fp3, 40(m), 0, 0;
ps_mul fp4, fp2, fp0;
psq_st fp12, 4(out), 1, 0;
ps_madd fp5, fp3, fp1, fp4;
ps_sum0 fp6, fp5, fp6, fp5;
psq_st fp6, 8(out), 1, 0;
blr
#endif // clang-format on
}
asm void PSMTXMultVecArray(register const Mtx m, register const Vec* srcBase, register Vec* dstBase,
register u32 count) {
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f13, 0(m), 0, 0
psq_l f12, 16(m), 0, 0
addi count, count, -1
psq_l f11, 8(m), 0, 0
ps_merge00 f0, f13, f12
addi dstBase, dstBase, -4
psq_l f10, 24(m), 0, 0
ps_merge11 f1, f13, f12
mtctr count
psq_l f4, 32(m), 0, 0
ps_merge00 f2, f11, f10
psq_l f5, 40(m), 0, 0
ps_merge11 f3, f11, f10
psq_l f6, 0(srcBase), 0, 0
psq_lu f7, 8(srcBase), 1, 0
ps_madds0 f8, f0, f6, f3
ps_mul f9, f4, f6
ps_madds1 f8, f1, f6, f8
ps_madd f10, f5, f7, f9
lbl_80346E0C:
psq_lu f6, 4(srcBase), 0, 0
ps_madds0 f12, f2, f7, f8
psq_lu f7, 8(srcBase), 1, 0
ps_sum0 f13, f10, f9, f10
ps_madds0 f8, f0, f6, f3
ps_mul f9, f4, f6
psq_stu f12, 4(dstBase), 0, 0
ps_madds1 f8, f1, f6, f8
psq_stu f13, 8(dstBase), 1, 0
ps_madd f10, f5, f7, f9
bdnz lbl_80346E0C
ps_madds0 f12, f2, f7, f8
ps_sum0 f13, f10, f9, f10
psq_stu f12, 4(dstBase), 0, 0
psq_stu f13, 8(dstBase), 1, 0
blr
#endif // clang-format on
}
asm void PSMTXMultVecSR(const register Mtx mtx, const register Vec* in, register Vec* out) {
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l fp0, 0(mtx), 0, 0;
psq_l fp6, 0(in), 0, 0;
psq_l fp2, 0x10(mtx), 0, 0;
ps_mul fp8, fp0, fp6;
psq_l fp4, 0x20(mtx), 0, 0;
ps_mul fp10, fp2, fp6;
psq_l fp7, 8(in), 1, 0;
ps_mul fp12, fp4, fp6;
psq_l fp3, 0x18(mtx), 0, 0;
ps_sum0 fp8, fp8, fp8, fp8;
psq_l fp5, 0x28(mtx), 0, 0;
ps_sum0 fp10, fp10, fp10, fp10;
psq_l fp1, 8(mtx), 0, 0;
ps_sum0 fp12, fp12, fp12, fp12;
ps_madd fp9, fp1, fp7, fp8;
psq_st fp9, 0(out), 1, 0;
ps_madd fp11, fp3, fp7, fp10;
psq_st fp11, 4(out), 1, 0;
ps_madd fp13, fp5, fp7, fp12;
psq_st fp13, 8(out), 1, 0;
blr
#endif // clang-format on
}
asm void PSMTXMultVecArraySR(register const Mtx m, register const Vec* srcBase,
register Vec* dstBase, register u32 count) {
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f13, 0(m), 0, 0
psq_l f12, 16(m), 0, 0
addi count, count, -1
psq_l f11, 8(m), 1, 0
ps_merge00 f0, f13, f12
addi dstBase, dstBase, -4
psq_l f10, 24(m), 1, 0
ps_merge11 f1, f13, f12
mtctr count
psq_l f3, 32(m), 0, 0
ps_merge00 f2, f11, f10
psq_l f4, 40(m), 1, 0
psq_l f6, 0(srcBase), 0, 0
psq_lu f7, 8(srcBase), 1, 0
ps_muls0 f8, f0, f6
ps_mul f9, f3, f6
ps_madds1 f8, f1, f6, f8
ps_madd f10, f4, f7, f9
lbl_80346EE8:
psq_lu f6, 4(srcBase), 0, 0
ps_madds0 f12, f2, f7, f8
psq_lu f7, 8(srcBase), 1, 0
ps_sum0 f13, f10, f9, f9
ps_muls0 f8, f0, f6
ps_mul f9, f3, f6
psq_stu f12, 4(dstBase), 0, 0
ps_madds1 f8, f1, f6, f8
psq_stu f13, 8(dstBase), 1, 0
ps_madd f10, f4, f7, f9
bdnz lbl_80346EE8
ps_madds0 f12, f2, f7, f8
ps_sum0 f13, f10, f9, f9
psq_stu f12, 4(dstBase), 0, 0
psq_stu f13, 8(dstBase), 1, 0
blr
#endif // clang-format on
}

355
src/dolphin/mtx/psmtx.c Normal file
View file

@ -0,0 +1,355 @@
#include <dolphin.h>
#include <dolphin/mtx.h>
#define qr0 0
#define qr1 1
#define qr6 6
asm void PSMTXReorder(const register Mtx src, register ROMtx dest)
{
/* clang-format off */
psq_l f0, 0(src), 0, qr0
psq_l f2, 16(src), 0, qr0
psq_l f4, 32(src), 0, qr0
psq_l f1, 8(src), 0, qr0
ps_merge00 f6, f0, f2
psq_l f3, 24(src), 0, qr0
ps_merge01 f12, f4, f0
psq_l f5, 40(src), 0, qr0
ps_merge11 f7, f2, f4
psq_st f6, 0(dest), 0, qr0
ps_merge00 f8, f1, f3
psq_st f12, 8(dest), 0, qr0
ps_merge01 f9, f5, f1
psq_st f7, 16(dest), 0, qr0
ps_merge11 f10, f3, f5
psq_st f8, 24(dest), 0, qr0
psq_st f9, 32(dest), 0, qr0
psq_st f10, 40(dest), 0, qr0
/* clang-format on */
}
asm void PSMTXROMultVecArray(const register ROMtx m, const register Vec *srcBase, register Vec *dstBase, register u32 count)
{
/* clang-format off */
nofralloc
stwu r1, -64(r1)
stfd f14, 8(r1)
subi r7, count, 1
stfd f15, 16(r1)
srwi r7, r7, 1
stfd f16, 24(r1)
stfd f17, 32(r1)
stfd f18, 40(r1)
mtctr r7
psq_l f0, 0(m), 0, qr0
subi srcBase, srcBase, 8
psq_l f1, 8(m), 1, qr0
subi dstBase, dstBase, 4
psq_l f6, 36(m), 0, qr0
psq_lu f8, 8(srcBase), 0, qr0
psq_l f7, 44(m), 1, qr0
psq_lu f9, 8(srcBase), 0, qr0
ps_madds0 f11, f0, f8, f6
psq_l f2, 12(m), 0, qr0
ps_madds0 f12, f1, f8, f7
psq_l f3, 20(m), 1, qr0
ps_madds1 f13, f0, f9, f6
psq_lu f10, 8(srcBase), 0, qr0
ps_madds1 f14, f1, f9, f7
psq_l f5, 32(m), 1, qr0
ps_madds1 f11, f2, f8, f11
ps_madds1 f12, f3, f8, f12
psq_l f4, 24(m), 0, qr0
ps_madds0 f13, f2, f10, f13
psq_lu f8, 8(srcBase), 0, qr0
ps_madds0 f14, f3, f10, f14
ps_madds0 f15, f4, f9, f11
ps_madds0 f16, f5, f9, f12
psq_lu f9, 8(srcBase), 0, qr0
ps_madds1 f17, f4, f10, f13
ps_madds1 f18, f5, f10, f14
psq_lu f10, 8(srcBase), 0, qr0
loop:
ps_madds0 f11, f0, f8, f6
psq_stu f15, 4(dstBase), 0, qr0
ps_madds0 f12, f1, f8, f7
psq_stu f16, 8(dstBase), 1, qr0
ps_madds1 f13, f0, f9, f6
psq_stu f17, 4(dstBase), 0, qr0
ps_madds1 f14, f1, f9, f7
psq_stu f18, 8(dstBase), 1, qr0
ps_madds1 f11, f2, f8, f11
ps_madds1 f12, f3, f8, f12
psq_lu f8, 8(srcBase), 0, qr0
ps_madds0 f13, f2, f10, f13
ps_madds0 f14, f3, f10, f14
ps_madds0 f15, f4, f9, f11
ps_madds0 f16, f5, f9, f12
psq_lu f9, 8(srcBase), 0, qr0
ps_madds1 f17, f4, f10, f13
ps_madds1 f18, f5, f10, f14
psq_lu f10, 8(srcBase), 0, qr0
bdnz loop
psq_stu f15, 4(dstBase), 0, qr0
clrlwi. r7, count, 31
psq_stu f16, 8(dstBase), 1, qr0
bne exit
psq_stu f17, 4(dstBase), 0, qr0
psq_stu f18, 8(dstBase), 1, qr0
exit:
lfd f14, 8(r1)
lfd f15, 16(r1)
lfd f16, 24(r1)
lfd f17, 32(r1)
lfd f18, 40(r1)
addi r1, r1, 64
blr
/* clang-format on */
}
asm void PSMTXROSkin2VecArray(const register ROMtx m0, const register ROMtx m1, const register f32 *wtBase, const register Vec *srcBase,
register Vec *dstBase, register u32 count)
{
/* clang-format off */
nofralloc
stwu r1, -160(r1)
stfd f14, 8(r1)
stfd f15, 16(r1)
stfd f16, 24(r1)
stfd f17, 32(r1)
stfd f18, 40(r1)
stfd f19, 48(r1)
stfd f20, 56(r1)
stfd f21, 64(r1)
stfd f22, 72(r1)
stfd f23, 80(r1)
stfd f24, 88(r1)
stfd f25, 96(r1)
stfd f26, 104(r1)
stfd f27, 112(r1)
stfd f28, 120(r1)
stfd f29, 128(r1)
stfd f30, 136(r1)
subi r9, r8, 1
mtctr r9
subi srcBase, srcBase, 4
subi dstBase, dstBase, 4
subi wtBase, wtBase, 4
psq_l f14, 0(m0), 0, qr0
psq_l f22, 0(m1), 0, qr0
psq_l f15, 8(m0), 1, qr0
psq_l f23, 8(m1), 1, qr0
psq_l f16, 12(m0), 0, qr0
psq_l f24, 12(m1), 0, qr0
ps_sub f22, f22, f14
psq_l f17, 20(m0), 1, qr0
psq_l f25, 20(m1), 1, qr0
ps_sub f23, f23, f15
psq_l f18, 24(m0), 0, qr0
psq_l f26, 24(m1), 0, qr0
ps_sub f24, f24, f16
psq_l f19, 32(m0), 1, qr0
psq_l f27, 32(m1), 1, qr0
ps_sub f25, f25, f17
psq_l f20, 36(m0), 0, qr0
psq_l f28, 36(m1), 0, qr0
ps_sub f26, f26, f18
psq_l f21, 44(m0), 1, qr0
psq_l f29, 44(m1), 1, qr0
ps_sub f27, f27, f19
ps_sub f28, f28, f20
ps_sub f29, f29, f21
psq_lu f30, 4(wtBase), 1, qr0
psq_lu f8, 4(srcBase), 0, qr0
psq_lu f9, 8(srcBase), 1, qr0
ps_madds0 f0, f22, f30, f14
ps_madds0 f1, f23, f30, f15
ps_madds0 f2, f24, f30, f16
ps_madds0 f3, f25, f30, f17
ps_madds0 f4, f26, f30, f18
ps_madds0 f5, f27, f30, f19
ps_madds0 f6, f28, f30, f20
ps_madds0 f7, f29, f30, f21
ps_madds0 f12, f0, f8, f6
ps_madds0 f13, f1, f8, f7
psq_lu f30, 4(wtBase), 1, qr0
loop:
ps_madds1 f12, f2, f8, f12
ps_madds1 f13, f3, f8, f13
psq_lu f8, 4(srcBase), 0, qr0
ps_madds0 f10, f4, f9, f12
ps_madds0 f11, f5, f9, f13
psq_lu f9, 8(srcBase), 1, qr0
ps_madds0 f0, f22, f30, f14
ps_madds0 f1, f23, f30, f15
ps_madds0 f2, f24, f30, f16
ps_madds0 f3, f25, f30, f17
ps_madds0 f4, f26, f30, f18
ps_madds0 f5, f27, f30, f19
ps_madds0 f6, f28, f30, f20
ps_madds0 f7, f29, f30, f21
psq_stu f10, 4(dstBase), 0, qr0
ps_madds0 f12, f0, f8, f6
ps_madds0 f13, f1, f8, f7
psq_stu f11, 8(dstBase), 1, qr0
psq_lu f30, 4(wtBase), 1, qr0
bdnz loop
ps_madds1 f12, f2, f8, f12
ps_madds1 f13, f3, f8, f13
ps_madds0 f10, f4, f9, f12
psq_stu f10, 4(dstBase), 0, qr0
ps_madds0 f11, f5, f9, f13
psq_stu f11, 8(dstBase), 1, qr0
lfd f14, 8(r1)
lfd f15, 16(r1)
lfd f16, 24(r1)
lfd f17, 32(r1)
lfd f18, 40(r1)
lfd f19, 48(r1)
lfd f20, 56(r1)
lfd f21, 64(r1)
lfd f22, 72(r1)
lfd f23, 80(r1)
lfd f24, 88(r1)
lfd f25, 96(r1)
lfd f26, 104(r1)
lfd f27, 112(r1)
lfd f28, 120(r1)
lfd f29, 128(r1)
lfd f30, 136(r1)
addi r1, r1, 160
blr
/* clang-format on */
}
asm void PSMTXROMultS16VecArray(const register ROMtx m, const register S16Vec *srcBase, register Vec *dstBase, register u32 count)
{
/* clang-format off */
nofralloc
stwu r1, -64(r1)
stfd f14, 8(r1)
subi r7, count, 1
stfd f15, 16(r1)
srwi r7, r7, 1
stfd f16, 24(r1)
lis r8, 7
stfd f17, 32(r1)
mtspr GQR6, r8
stfd f18, 40(r1)
mtctr r7
psq_l f0, 0(m), 0, qr0
subi srcBase, srcBase, 4
psq_l f1, 8(m), 1, qr0
subi dstBase, dstBase, 4
psq_l f6, 36(m), 0, qr0
psq_lu f8, 4(srcBase), 0, qr6
psq_l f7, 44(m), 1, qr0
psq_lu f9, 4(srcBase), 0, qr6
ps_madds0 f11, f0, f8, f6
psq_l f2, 12(m), 0, qr0
ps_madds0 f12, f1, f8, f7
psq_l f3, 20(m), 1, qr0
ps_madds1 f13, f0, f9, f6
psq_lu f10, 4(srcBase), 0, qr6
ps_madds1 f14, f1, f9, f7
psq_l f5, 32(m), 1, qr0
ps_madds1 f11, f2, f8, f11
ps_madds1 f12, f3, f8, f12
psq_l f4, 24(m), 0, qr0
ps_madds0 f13, f2, f10, f13
psq_lu f8, 4(srcBase), 0, qr6
ps_madds0 f14, f3, f10, f14
ps_madds0 f15, f4, f9, f11
ps_madds0 f16, f5, f9, f12
psq_lu f9, 4(srcBase), 0, qr6
ps_madds1 f17, f4, f10, f13
ps_madds1 f18, f5, f10, f14
psq_lu f10, 4(srcBase), 0, qr6
loop:
ps_madds0 f11, f0, f8, f6
psq_stu f15, 4(dstBase), 0, qr0
ps_madds0 f12, f1, f8, f7
psq_stu f16, 8(dstBase), 1, qr0
ps_madds1 f13, f0, f9, f6
psq_stu f17, 4(dstBase), 0, qr0
ps_madds1 f14, f1, f9, f7
psq_stu f18, 8(dstBase), 1, qr0
ps_madds1 f11, f2, f8, f11
ps_madds1 f12, f3, f8, f12
psq_lu f8, 4(srcBase), 0, qr6
ps_madds0 f13, f2, f10, f13
ps_madds0 f14, f3, f10, f14
ps_madds0 f15, f4, f9, f11
ps_madds0 f16, f5, f9, f12
psq_lu f9, 4(srcBase), 0, qr6
ps_madds1 f17, f4, f10, f13
ps_madds1 f18, f5, f10, f14
psq_lu f10, 4(srcBase), 0, qr6
bdnz loop
psq_stu f15, 4(dstBase), 0, qr0
clrlwi. r7, count, 31
psq_stu f16, 8(dstBase), 1, qr0
bne exit
psq_stu f17, 4(dstBase), 0, qr0
psq_stu f18, 8(dstBase), 1, qr0
exit:
lfd f14, 8(r1)
lfd f15, 16(r1)
lfd f16, 24(r1)
lfd f17, 32(r1)
lfd f18, 40(r1)
addi r1, r1, 64
blr
/* clang-format on */
}
asm void PSMTXMultS16VecArray(const register Mtx44 m, const register S16Vec *srcBase, register Vec *dstBase, register u32 count)
{
/* clang-format off */
psq_l f0, 0(m), 0, qr0
lis r7, 7
mtspr GQR6, r7
psq_l f6, 0(srcBase), 0, qr6
subi count, count, 1
psq_l f7, 4(srcBase), 1, qr6
mtctr count
psq_l f1, 8(m), 0, qr0
addi srcBase, srcBase, 4
psq_l f2, 16(m), 0, qr0
subi dstBase, dstBase, 4
psq_l f3, 24(m), 0, qr0
ps_mul f8, f0, f6
psq_l f4, 32(m), 0, qr0
ps_mul f10, f2, f6
psq_l f5, 40(m), 0, qr0
ps_mul f12, f4, f6
psq_lu f6, 2(srcBase), 0, qr1
ps_madd f8, f1, f7, f8
ps_madd f10, f3, f7, f10
ps_madd f12, f5, f7, f12
psq_lu f7, 4(srcBase), 1, qr6
ps_sum0 f9, f8, f8, f8
loop:
ps_sum0 f11, f10, f10, f10
ps_mul f8, f0, f6
ps_sum0 f13, f12, f12, f12
ps_mul f10, f2, f6
psq_stu f9, 4(dstBase), 1, qr0
ps_mul f12, f4, f6
psq_stu f11, 4(dstBase), 1, qr0
ps_madd f8, f1, f7, f8
psq_stu f13, 4(dstBase), 1, qr0
ps_madd f10, f3, f7, f10
psq_lu f6, 2(srcBase), 0, qr6
ps_madd f12, f5, f7, f12
psq_lu f7, 4(srcBase), 1, qr6
ps_sum0 f9, f8, f8, f8
bdnz loop
ps_sum0 f11, f10, f10, f10
ps_sum0 f13, f12, f12, f12
psq_stu f9, 4(dstBase), 1, qr0
psq_stu f11, 4(dstBase), 1, qr0
psq_stu f13, 4(dstBase), 1, qr0
/* clang-format on */
}

80
src/dolphin/mtx/quat.c Normal file
View file

@ -0,0 +1,80 @@
#include "dolphin/mtx.h"
#include "math.h"
void PSQUATMultiply(register const Quaternion *a, register const Quaternion *b, register Quaternion *ab)
{
asm {
psq_l f0, 0(a), 0, 0
psq_l f1, 8(a), 0, 0
psq_l f2, 0(b), 0, 0
ps_neg f5, f0
psq_l f3, 8(b), 0, 0
ps_neg f6, f1
ps_merge01 f4, f5, f0
ps_muls0 f7, f1, f2
ps_muls0 f5, f5, f2
ps_merge01 f1, f6, f1
ps_muls1 f8, f4, f2
ps_madds0 f7, f4, f3, f7
ps_muls1 f2, f1, f2
ps_madds0 f5, f1, f3, f5
ps_madds1 f8, f6, f3, f8
ps_merge10 f7, f7, f7
ps_madds1 f2, f0, f3, f2
ps_merge10 f5, f5, f5
ps_add f7, f7, f2
psq_st f7, 0(ab), 0, 0
ps_sub f5, f5, f8
psq_st f5, 8(ab), 0, 0
}
}
void C_QUATRotAxisRad(Quaternion *q, const Vec *axis, f32 rad)
{
f32 tmp, tmp2, tmp3;
Vec dst;
tmp = rad;
PSVECNormalize(axis, &dst);
tmp2 = tmp * 0.5f;
tmp3 = sinf(tmp * 0.5f);
tmp = tmp3;
tmp3 = cosf(tmp2);
q->x = tmp * dst.x;
q->y = tmp * dst.y;
q->z = tmp * dst.z;
q->w = tmp3;
}
void C_QUATSlerp(const Quaternion *p, const Quaternion *q, Quaternion *r, f32 t)
{
f32 ratioA, ratioB;
f32 value = 1.0f;
f32 cosHalfTheta = p->x * q->x + p->y * q->y + p->z * q->z + p->w * q->w;
if (cosHalfTheta < 0.0f) {
cosHalfTheta = -cosHalfTheta;
value = -value;
}
if (cosHalfTheta <= 0.9999899864196777f) {
f32 halfTheta = acosf(cosHalfTheta);
f32 sinHalfTheta = sinf(halfTheta);
ratioA = sinf((1.0f - t) * halfTheta) / sinHalfTheta;
ratioB = sinf(t * halfTheta) / sinHalfTheta;
value *= ratioB;
}
else {
ratioA = 1.0f - t;
value *= t;
}
r->x = (ratioA * p->x) + (value * q->x);
r->y = (ratioA * p->y) + (value * q->y);
r->z = (ratioA * p->z) + (value * q->z);
r->w = (ratioA * p->w) + (value * q->w);
}

287
src/dolphin/mtx/vec.c Normal file
View file

@ -0,0 +1,287 @@
#include "dolphin/mtx.h"
#include "math.h"
#define R_RET fp1
#define FP2 fp2
#define FP3 fp3
#define FP4 fp4
#define FP5 fp5
#define FP6 fp6
#define FP7 fp7
#define FP8 fp8
#define FP9 fp9
#define FP10 fp10
#define FP11 fp11
#define FP12 fp12
#define FP13 fp13
asm void PSVECAdd(const register Vec *vec1, const register Vec *vec2, register Vec *ret)
{
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l FP2, 0(vec1), 0, 0;
psq_l FP4, 0(vec2), 0, 0;
ps_add FP6, FP2, FP4;
psq_st FP6, 0(ret), 0, 0;
psq_l FP3, 8(vec1), 1, 0;
psq_l FP5, 8(vec2), 1, 0;
ps_add FP7, FP3, FP5;
psq_st FP7, 8(ret), 1, 0;
blr
#endif // clang-format on
}
asm void PSVECSubtract(const register Vec *vec1, const register Vec *vec2, register Vec *ret)
{
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l FP2, 0(vec1), 0, 0;
psq_l FP4, 0(vec2), 0, 0;
ps_sub FP6, FP2, FP4;
psq_st FP6, 0(ret), 0, 0;
psq_l FP3, 8(vec1), 1, 0;
psq_l FP5, 8(vec2), 1, 0;
ps_sub FP7, FP3, FP5;
psq_st FP7, 8(ret), 1, 0;
blr
#endif // clang-format on
}
asm void PSVECScale(register const Vec *src, register Vec *dst, register f32 scale)
{
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f0, 0(src), 0, 0
psq_l f2, 8(src), 1, 0
ps_muls0 f0, f0, f1
psq_st f0, 0(dst), 0, 0
ps_muls0 f0, f2, f1
psq_st f0, 8(dst), 1, 0
blr
#endif // clang-format on
}
void C_VECScale(const Vec *src, Vec *dst, f32 scale)
{
f32 s;
s = 1.0f / sqrtf(src->z * src->z + src->x * src->x + src->y * src->y);
dst->x = src->x * s;
dst->y = src->y * s;
dst->z = src->z * s;
}
void PSVECNormalize(const register Vec *vec1, register Vec *ret)
{
#ifdef __MWERKS__ // clang-format off
register f32 half = 0.5f;
register f32 three = 3.0f;
register f32 xx_zz, xx_yy;
register f32 square_sum;
register f32 ret_sqrt;
register f32 n_0, n_1;
asm {
psq_l FP2, 0(vec1), 0, 0;
ps_mul xx_yy, FP2, FP2;
psq_l FP3, 8(vec1), 1, 0;
ps_madd xx_zz, FP3, FP3, xx_yy;
ps_sum0 square_sum, xx_zz, FP3, xx_yy;
frsqrte ret_sqrt, square_sum;
fmuls n_0, ret_sqrt, ret_sqrt;
fmuls n_1, ret_sqrt, half;
fnmsubs n_0, n_0, square_sum, three;
fmuls ret_sqrt, n_0, n_1;
ps_muls0 FP2, FP2, ret_sqrt;
psq_st FP2, 0(ret), 0, 0;
ps_muls0 FP3, FP3, ret_sqrt;
psq_st FP3, 8(ret), 1, 0;
}
#endif // clang-format on
}
asm f32 PSVECSquareMag(register const Vec *v) {
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f0, 0(v), 0, 0
ps_mul f0, f0, f0
lfs f1, 8(v)
ps_madd f1, f1, f1, f0
ps_sum0 f1, f1, f0, f0
blr
#endif // clang-format on
}
f32 PSVECMag(const register Vec *v)
{
register f32 v_xy, v_zz, square_mag;
register f32 ret_mag, n_0, n_1;
register f32 three, half, zero;
#ifdef __MWERKS__ // clang-format off
asm {
psq_l v_xy, 0(v), 0, 0
ps_mul v_xy, v_xy, v_xy
lfs v_zz, 8(v)
ps_madd square_mag, v_zz, v_zz, v_xy
}
#endif // clang-format on
half = 0.5f;
#ifdef __MWERKS__ // clang-format off
asm {
ps_sum0 square_mag, square_mag, v_xy, v_xy
frsqrte ret_mag, square_mag
}
#endif // clang-format on
three = 3.0f;
#ifdef __MWERKS__ // clang-format off
asm {
fmuls n_0, ret_mag, ret_mag
fmuls n_1, ret_mag, half
fnmsubs n_0, n_0, square_mag, three
fmuls ret_mag, n_0, n_1
fsel ret_mag, ret_mag, ret_mag, square_mag
fmuls square_mag, square_mag, ret_mag
}
#endif // clang-format on
return square_mag;
}
asm f32 PSVECDotProduct(const register Vec *vec1, const register Vec *vec2)
{
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l f2, 4(r3), 0, 0 /* qr0 */
psq_l f3, 4(r4), 0, 0 /* qr0 */
ps_mul f2, f2, f3
psq_l f5, 0(r3), 0, 0 /* qr0 */
psq_l f4, 0(r4), 0, 0 /* qr0 */
ps_madd f3, f5, f4, f2
ps_sum0 f1, f3, f2, f2
blr
#endif // clang-format on
}
asm void PSVECCrossProduct(register const Vec *a, register const Vec *b, register Vec *axb)
{
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f1, 0(b), 0, 0
lfs f2, 8(a)
psq_l f0, 0(a), 0, 0
ps_merge10 f6, f1, f1
lfs f3, 8(b)
ps_mul f4, f1, f2
ps_muls0 f7, f1, f0
ps_msub f5, f0, f3, f4
ps_msub f8, f0, f6, f7
ps_merge11 f9, f5, f5
ps_merge01 f10, f5, f8
psq_st f9, 0(axb), 1, 0
ps_neg f10, f10
psq_st f10, 4(axb), 0, 0
blr
#endif // clang-format on
}
void C_VECHalfAngle(const Vec *a, const Vec *b, Vec *half)
{
Vec a0;
Vec b0;
Vec ab;
a0.x = -a->x;
a0.y = -a->y;
a0.z = -a->z;
b0.x = -b->x;
b0.y = -b->y;
b0.z = -b->z;
VECNormalize(&a0, &a0);
VECNormalize(&b0, &b0);
VECAdd(&a0, &b0, &ab);
if (VECDotProduct(&ab, &ab) > 0.0f) {
VECNormalize(&ab, half);
}
else {
*half = ab;
}
}
void C_VECReflect(const Vec *src, const Vec *normal, Vec *dst)
{
// Vec a0;
// Vec b0;
// f32 dot;
// a0.x = -src->x;
// a0.y = -src->y;
// a0.z = -src->z;
// VECNormalize(&a0, &a0);
// VECNormalize(normal, &b0);
// dot = VECDotProduct(&a0, &b0);
// dst->x = b0.x * 2.0f * dot - a0.x;
// dst->y = b0.y * 2.0f * dot - a0.y;
// dst->z = b0.z * 2.0f * dot - a0.z;
// VECNormalize(dst, dst);
}
asm f32 PSVECSquareDistance(register const Vec *a, register const Vec *b) {
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f0, 4(a), 0, 0
psq_l f1, 4(b), 0, 0
ps_sub f2, f0, f1
psq_l f0, 0(a), 0, 0
psq_l f1, 0(b), 0, 0
ps_mul f2, f2, f2
ps_sub f0, f0, f1
ps_madd f1, f0, f0, f2
ps_sum0 f1, f1, f2, f2
blr
#endif // clang-format on
}
f32 PSVECDistance(register const Vec *a, register const Vec *b)
{
register f32 half_c;
register f32 three_c;
register f32 dist;
#ifdef __MWERKS__ // clang-format off
asm {
psq_l f0, 4(a), 0, 0 /* qr0 */
psq_l f1, 4(b), 0, 0 /* qr0 */
ps_sub f2, f0, f1
psq_l f0, 0(a), 0, 0 /* qr0 */
psq_l f1, 0(b), 0, 0 /* qr0 */
ps_mul f2, f2, f2
ps_sub f0, f0, f1
}
half_c = 0.5f;
asm {
ps_madd f0, f0, f0, f2
ps_sum0 f0, f0, f2, f2
}
three_c = 3.0f;
asm {
frsqrte dist, f0
fmuls f2, dist, dist
fmuls dist, dist, half_c
fnmsubs f2, f2, f0, three_c
fmuls dist, f2, dist
fsel dist, dist, dist, f0
fmuls dist, f0, dist
}
return dist;
#endif // clang-format on
}

783
src/dolphin/pad/Pad.c Normal file
View file

@ -0,0 +1,783 @@
#include <dolphin/pad.h>
#include <dolphin/sipriv.h>
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;
}
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);
}
}
PADSamplingCallback PADSetSamplingCallback(PADSamplingCallback callback)
{
PADSamplingCallback prev;
prev = SamplingCallback;
SamplingCallback = callback;
if (callback) {
SIRegisterPollingHandler(SamplingHandler);
}
else {
SIUnregisterPollingHandler(SamplingHandler);
}
return prev;
}
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;
}

119
src/dolphin/pad/Padclamp.c Normal file
View file

@ -0,0 +1,119 @@
#include <dolphin/pad.h>
#include <math.h>
typedef struct PADClampRegion {
u8 minTrigger;
u8 maxTrigger;
s8 minStick;
s8 maxStick;
s8 xyStick;
s8 minSubstick;
s8 maxSubstick;
s8 xySubstick;
} PADClampRegion;
static PADClampRegion ClampRegion = {
// Triggers
30,
180,
// Left stick
15,
72,
40,
// Right stick
15,
59,
31,
};
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);
// }
}