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
|
#include "ultra64.h"
#include "PR/os_motor.h"
#include "PR/controller.h"
#include "alignment.h"
#define BANK_ADDR 0x400
#define MOTOR_ID 0x80
OSPifRam __MotorDataBuf[MAXCONTROLLERS] ALIGNED(16);
s32 __osPfsSelectBank(OSPfs* pfs, u8 bank);
s32 __osMotorAccess(OSPfs* pfs, s32 flag) {
s32 i;
s32 ret;
u8* ptr = (u8*)&__MotorDataBuf[pfs->channel];
if (!(pfs->status & PFS_MOTOR_INITIALIZED)) {
return PFS_ERR_INVALID;
}
__osSiGetAccess();
__MotorDataBuf[pfs->channel].status = CONT_CMD_EXE;
ptr += pfs->channel;
for (i = 0; i < BLOCKSIZE; i++) {
READFORMAT(ptr)->data[i] = flag;
}
__osContLastPoll = CONT_CMD_END;
__osSiRawStartDma(OS_WRITE, &__MotorDataBuf[pfs->channel]);
osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK);
__osSiRawStartDma(OS_READ, &__MotorDataBuf[pfs->channel]);
osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK);
ret = READFORMAT(ptr)->rxsize & CHNL_ERR_MASK;
if (!ret) {
if (!flag) {
if (READFORMAT(ptr)->datacrc != 0) {
ret = PFS_ERR_CONTRFAIL;
}
} else {
if (READFORMAT(ptr)->datacrc != 0xEB) {
ret = PFS_ERR_CONTRFAIL;
}
}
}
__osSiRelAccess();
return ret;
}
void __osMakeMotorData(s32 channel, OSPifRam* mdata) {
u8* ptr = (u8*)mdata->ramarray;
__OSContRamReadFormat ramreadformat;
s32 i;
ramreadformat.dummy = CONT_CMD_NOP;
ramreadformat.txsize = CONT_CMD_WRITE_PAK_TX;
ramreadformat.rxsize = CONT_CMD_WRITE_PAK_RX;
ramreadformat.cmd = CONT_CMD_WRITE_PAK;
ramreadformat.hi = CONT_BLOCK_RUMBLE >> 3;
ramreadformat.lo = (u8)(__osContAddressCrc(CONT_BLOCK_RUMBLE) | (CONT_BLOCK_RUMBLE << 5));
if (channel != 0) {
for (i = 0; i < channel; i++) {
*ptr++ = 0;
}
}
*READFORMAT(ptr) = ramreadformat;
ptr += sizeof(ramreadformat);
*ptr = CONT_CMD_END;
}
s32 osMotorInit(OSMesgQueue* mq, OSPfs* pfs, s32 channel) {
s32 ret;
u8 temp[BLOCKSIZE];
pfs->queue = mq;
pfs->channel = channel;
pfs->activebank = 0xFF;
pfs->status = 0;
ret = __osPfsSelectBank(pfs, 0xFE);
if (ret == PFS_ERR_NEW_PACK) {
ret = __osPfsSelectBank(pfs, MOTOR_ID);
}
if (ret != 0) {
return ret;
}
ret = __osContRamRead(mq, channel, CONT_BLOCK_DETECT, temp);
if (ret == PFS_ERR_NEW_PACK) {
ret = PFS_ERR_CONTRFAIL;
}
if (ret != 0) {
return ret;
}
if (temp[BLOCKSIZE - 1] == 0xFE) {
return PFS_ERR_DEVICE;
}
ret = __osPfsSelectBank(pfs, MOTOR_ID);
if (ret == 2) {
ret = PFS_ERR_CONTRFAIL;
}
if (ret != 0) {
return ret;
}
ret = __osContRamRead(mq, channel, BANK_ADDR, temp);
if (ret == PFS_ERR_NEW_PACK) {
ret = PFS_ERR_CONTRFAIL;
}
if (ret != 0) {
return ret;
}
if (temp[BLOCKSIZE - 1] != MOTOR_ID) {
return PFS_ERR_DEVICE;
}
if (!(pfs->status & PFS_MOTOR_INITIALIZED)) {
__osMakeMotorData(channel, &__MotorDataBuf[channel]);
}
pfs->status = PFS_MOTOR_INITIALIZED;
return 0;
}
|
