root/libcwiid/process.c

Revision b54bd057fe6b258d6e0a99047f839891ab3bc2df, 8.7 kB (checked in by L. Donnie Smith <donnie.smith@…>, 2 years ago)

Fixed an issue of incorrectly interpreting Balance Board as MotionPlus.

Previous extension identifiers were only one byte long (just considering
the byte 0xA400FE) and that byte of Balance Board and MotionPlus
happens to be identical (0x04).

In reality, extension identifier is six byte long, but as long as GHWT Drums
are not supported, checking the last two bytes is enough.

See http://wiibrew.org/wiki/Wiimote#The_New_Way for more information on
identifying extensions.
  • Property mode set to 100644
Line 
1/* Copyright (C) 2007 L. Donnie Smith <donnie.smith@gatech.edu>
2 *
3 *  This program is free software; you can redistribute it and/or modify
4 *  it under the terms of the GNU General Public License as published by
5 *  the Free Software Foundation; either version 2 of the License, or
6 *  (at your option) any later version.
7 *
8 *  This program is distributed in the hope that it will be useful,
9 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
10 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11 *  GNU General Public License for more details.
12 *
13 *  You should have received a copy of the GNU General Public License
14 *  along with this program; if not, write to the Free Software
15 *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
16 *
17 */
18
19#include <unistd.h>
20#include "cwiid_internal.h"
21
22int process_error(struct wiimote *wiimote, ssize_t len, struct mesg_array *ma)
23{
24        struct cwiid_error_mesg *error_mesg;
25
26        error_mesg = &ma->array[ma->count++].error_mesg;
27        error_mesg->type = CWIID_MESG_ERROR;
28        if (len == 0) {
29                error_mesg->error = CWIID_ERROR_DISCONNECT;
30        }
31        else {
32                error_mesg->error = CWIID_ERROR_COMM;
33        }
34
35        if (cancel_rw(wiimote)) {
36                cwiid_err(wiimote, "RW cancel error");
37        }
38
39        return 0;
40}
41
42int process_status(struct wiimote *wiimote, const unsigned char *data,
43                   struct mesg_array *ma)
44{
45        struct cwiid_status_mesg status_mesg;
46
47        status_mesg.type = CWIID_MESG_STATUS;
48        status_mesg.battery = data[5];
49        if (data[2] & 0x02) {
50                /* status_thread will figure out what it is */
51                status_mesg.ext_type = CWIID_EXT_UNKNOWN;
52        }
53        else {
54                status_mesg.ext_type = CWIID_EXT_NONE;
55        }
56
57        if (write(wiimote->status_pipe[1], &status_mesg, sizeof status_mesg)
58          != sizeof status_mesg) {
59                cwiid_err(wiimote, "Status pipe write error");
60                return -1;
61        }
62
63        return 0;
64}
65
66int process_btn(struct wiimote *wiimote, const unsigned char *data,
67                struct mesg_array *ma)
68{
69        struct cwiid_btn_mesg *btn_mesg;
70        uint16_t buttons;
71
72        buttons = (data[0] & BTN_MASK_0)<<8 |
73                  (data[1] & BTN_MASK_1);
74        if (wiimote->state.rpt_mode & CWIID_RPT_BTN) {
75                if ((wiimote->state.buttons != buttons) ||
76                  (wiimote->flags & CWIID_FLAG_REPEAT_BTN)) {
77                        btn_mesg = &ma->array[ma->count++].btn_mesg;
78                        btn_mesg->type = CWIID_MESG_BTN;
79                        btn_mesg->buttons = buttons;
80                }
81        }
82
83        return 0;
84}
85
86int process_acc(struct wiimote *wiimote, const unsigned char *data,
87                struct mesg_array *ma)
88{
89        struct cwiid_acc_mesg *acc_mesg;
90
91        if (wiimote->state.rpt_mode & CWIID_RPT_ACC) {
92                acc_mesg = &ma->array[ma->count++].acc_mesg;
93                acc_mesg->type = CWIID_MESG_ACC;
94                acc_mesg->acc[CWIID_X] = data[0];
95                acc_mesg->acc[CWIID_Y] = data[1];
96                acc_mesg->acc[CWIID_Z] = data[2];
97        }
98
99        return 0;
100}
101
102int process_ir10(struct wiimote *wiimote, const unsigned char *data,
103                 struct mesg_array *ma)
104{
105        struct cwiid_ir_mesg *ir_mesg;
106        int i;
107        const unsigned char *block;
108
109        if (wiimote->state.rpt_mode & CWIID_RPT_IR) {
110                ir_mesg = &ma->array[ma->count++].ir_mesg;
111                ir_mesg->type = CWIID_MESG_IR;
112
113                for (i=0, block=data; i < CWIID_IR_SRC_COUNT; i+=2, block+=5) {
114                        if (block[0] == 0xFF) {
115                                ir_mesg->src[i].valid = 0;
116                        }
117                        else {
118                                ir_mesg->src[i].valid = 1;
119                                ir_mesg->src[i].pos[CWIID_X] = ((uint16_t)block[2] & 0x30)<<4 |
120                                                                (uint16_t)block[0];
121                                ir_mesg->src[i].pos[CWIID_Y] = ((uint16_t)block[2] & 0xC0)<<2 |
122                                                                (uint16_t)block[1];
123                                ir_mesg->src[i].size = -1;
124                        }
125
126                        if (block[3] == 0xFF) {
127                                ir_mesg->src[i+1].valid = 0;
128                        }
129                        else {
130                                ir_mesg->src[i+1].valid = 1;
131                                ir_mesg->src[i+1].pos[CWIID_X] =
132                                                               ((uint16_t)block[2] & 0x03)<<8 |
133                                                                (uint16_t)block[3];
134                                ir_mesg->src[i+1].pos[CWIID_Y] =
135                                                               ((uint16_t)block[2] & 0x0C)<<6 |
136                                                                (uint16_t)block[4];
137                                ir_mesg->src[i+1].size = -1;
138                        }
139                }
140        }
141
142        return 0;
143}
144
145int process_ir12(struct wiimote *wiimote, const unsigned char *data,
146                 struct mesg_array *ma)
147{
148        struct cwiid_ir_mesg *ir_mesg;
149        int i;
150        const unsigned char *block;
151
152        if (wiimote->state.rpt_mode & CWIID_RPT_IR) {
153                ir_mesg = &ma->array[ma->count++].ir_mesg;
154                ir_mesg->type = CWIID_MESG_IR;
155
156                for (i=0, block=data; i < CWIID_IR_SRC_COUNT; i++, block+=3) {
157                        if (block[0] == 0xFF) {
158                                ir_mesg->src[i].valid = 0;
159                        }
160                        else {
161                                ir_mesg->src[i].valid = 1;
162                                ir_mesg->src[i].pos[CWIID_X] = ((uint16_t)block[2] & 0x30)<<4 |
163                                                                (uint16_t)block[0];
164                                ir_mesg->src[i].pos[CWIID_Y] = ((uint16_t)block[2] & 0xC0)<<2 |
165                                                                (uint16_t)block[1];
166                                ir_mesg->src[i].size = block[2] & 0x0F;
167                        }
168                }
169        }
170
171        return 0;
172}
173
174int process_ext(struct wiimote *wiimote, unsigned char *data,
175                unsigned char len, struct mesg_array *ma)
176{
177        struct cwiid_nunchuk_mesg *nunchuk_mesg;
178        struct cwiid_classic_mesg *classic_mesg;
179        struct cwiid_balance_mesg *balance_mesg;
180        struct cwiid_motionplus_mesg *motionplus_mesg;
181        int i;
182
183        switch (wiimote->state.ext_type) {
184        case CWIID_EXT_NONE:
185                cwiid_err(wiimote, "Received unexpected extension report");
186                break;
187        case CWIID_EXT_UNKNOWN:
188                break;
189        case CWIID_EXT_NUNCHUK:
190                if (wiimote->state.rpt_mode & CWIID_RPT_NUNCHUK) {
191                        nunchuk_mesg = &ma->array[ma->count++].nunchuk_mesg;
192                        nunchuk_mesg->type = CWIID_MESG_NUNCHUK;
193                        nunchuk_mesg->stick[CWIID_X] = data[0];
194                        nunchuk_mesg->stick[CWIID_Y] = data[1];
195                        nunchuk_mesg->acc[CWIID_X] = data[2];
196                        nunchuk_mesg->acc[CWIID_Y] = data[3];
197                        nunchuk_mesg->acc[CWIID_Z] = data[4];
198                        nunchuk_mesg->buttons = ~data[5] & NUNCHUK_BTN_MASK;
199                }
200                break;
201        case CWIID_EXT_CLASSIC:
202                if (wiimote->state.rpt_mode & CWIID_RPT_CLASSIC) {
203                        classic_mesg = &ma->array[ma->count++].classic_mesg;
204                        classic_mesg->type = CWIID_MESG_CLASSIC;
205
206                        for (i=0; i < 6; i++) {
207                                data[i] = data[i];
208                        }
209
210                        classic_mesg->l_stick[CWIID_X] = data[0] & 0x3F;
211                        classic_mesg->l_stick[CWIID_Y] = data[1] & 0x3F;
212                        classic_mesg->r_stick[CWIID_X] = (data[0] & 0xC0)>>3 |
213                                                         (data[1] & 0xC0)>>5 |
214                                                         (data[2] & 0x80)>>7;
215                        classic_mesg->r_stick[CWIID_Y] = data[2] & 0x1F;
216                        classic_mesg->l = (data[2] & 0x60)>>2 |
217                                          (data[3] & 0xE0)>>5;
218                        classic_mesg->r = data[3] & 0x1F;
219                        classic_mesg->buttons = ~((uint16_t)data[4]<<8 |
220                                                  (uint16_t)data[5]);
221                }
222                break;
223        case CWIID_EXT_BALANCE:
224                if (wiimote->state.rpt_mode & CWIID_RPT_BALANCE) {
225                        balance_mesg = &ma->array[ma->count++].balance_mesg;
226                        balance_mesg->type = CWIID_MESG_BALANCE;
227                        balance_mesg->right_top = ((uint16_t)data[0]<<8 |
228                                                   (uint16_t)data[1]);
229                        balance_mesg->right_bottom = ((uint16_t)data[2]<<8 |
230                                                      (uint16_t)data[3]);
231                        balance_mesg->left_top = ((uint16_t)data[4]<<8 |
232                                                  (uint16_t)data[5]);
233                        balance_mesg->left_bottom = ((uint16_t)data[6]<<8 |
234                                                     (uint16_t)data[7]);
235                }
236                break;
237        case CWIID_EXT_MOTIONPLUS:
238                if (wiimote->state.rpt_mode & CWIID_RPT_MOTIONPLUS) {
239                        motionplus_mesg = &ma->array[ma->count++].motionplus_mesg;
240                        motionplus_mesg->type = CWIID_MESG_MOTIONPLUS;
241                        motionplus_mesg->angle_rate[CWIID_PHI]   = ((uint16_t)data[5] & 0xFC)<<6 |
242                                                                    (uint16_t)data[2];
243                        motionplus_mesg->angle_rate[CWIID_THETA] = ((uint16_t)data[4] & 0xFC)<<6 |
244                                                                    (uint16_t)data[1];
245                        motionplus_mesg->angle_rate[CWIID_PSI]   = ((uint16_t)data[3] & 0xFC)<<6 |
246                                                                    (uint16_t)data[0];
247                        motionplus_mesg->low_speed[CWIID_PHI]    = ((uint8_t)data[3] & 0x01);
248                        motionplus_mesg->low_speed[CWIID_THETA]  = ((uint8_t)data[4] & 0x02)>>1;
249                        motionplus_mesg->low_speed[CWIID_PSI]    = ((uint8_t)data[3] & 0x02)>>1;
250                }
251                break;
252        }
253
254        return 0;
255}
256
257int process_read(struct wiimote *wiimote, unsigned char *data)
258{
259        struct rw_mesg rw_mesg;
260
261        if (wiimote->rw_status != RW_READ) {
262                cwiid_err(wiimote, "Received unexpected read report");
263                return -1;
264        }
265
266        rw_mesg.type = RW_READ;
267        rw_mesg.len = (data[0]>>4)+1;
268        rw_mesg.error = data[0] & 0x0F;
269        memcpy(&rw_mesg.data, data+3, rw_mesg.len);
270
271        if (write(wiimote->rw_pipe[1], &rw_mesg, sizeof rw_mesg) !=
272          sizeof rw_mesg) {
273                cwiid_err(wiimote, "RW pipe write error");
274                return -1;
275        }
276
277        return 0;
278}
279
280int process_write(struct wiimote *wiimote, unsigned char *data)
281{
282        struct rw_mesg rw_mesg;
283
284        if (wiimote->rw_status != RW_WRITE) {
285                cwiid_err(wiimote, "Received unexpected write report");
286                return -1;
287        }
288
289        rw_mesg.type = RW_WRITE;
290        rw_mesg.error = data[0];
291
292        if (write(wiimote->rw_pipe[1], &rw_mesg, sizeof rw_mesg) !=
293          sizeof rw_mesg) {
294                cwiid_err(wiimote, "RW pipe write error");
295                return -1;
296        }
297
298        return 0;
299}
Note: See TracBrowser for help on using the browser.