Context Navigation

← Previous Changeset
Next Changeset →

Changeset 00aa4948876dc0530d8a1e5ab8170a9befa9f14c

Timestamp:

03/20/07 00:49:37 (6 years ago)

Author:

dsmith <dsmith@…>

Children:

83478a060418b505c3358014735bf538b715c520

Parents:

e3670b785a5a2f9d83bf41145b470893f2901e55

git-author:

L. Donnie Smith <donnie.smith@…> (03/20/07 00:49:37)

git-committer:

dsmith <dsmith@…> (03/20/07 00:49:37)

Message:

wiimote: error checking, comment updates

git-svn-id: http://abstrakraft.org/cwiid/svn/trunk@35 918edb2d-ff29-0410-9de2-eb38e7f22bc7

Files:

: 7 modified

ChangeLog (modified) (1 diff)
wiimote/command.c (modified) (7 diffs)
wiimote/connect.c (modified) (7 diffs)
wiimote/event.c (modified) (13 diffs)
wiimote/queue.c (modified) (2 diffs)
wiimote/rw.c (modified) (2 diffs)
wiimote/util.c (modified) (5 diffs)

Legend:

: Unmodified
: Added
: Removed

ChangeLog

r9876aed	r00aa494
1	1	2007-03-19 L. Donnie Smith <cwiid@abstrakraft.org>
2	2	* Added --disable-ldconfig option to configure
	3	* Makefiles - append to rather than set compiler options
3	4
4	5	2007-03-15 L. Donnie Smith <cwiid@abstrakraft.org>
5	6	wminput
6	7	* action_enum.awk - explicitly call awk rather than #! /usr/bin/awk
	8
	9	2007-03-14 L. Donnie Smith <cwiid@abstrakraft.org>
	10	wiimote
	11	* audited error checking (coda and error handler sections)
	12	* updated comments
	13	* event.c - moved int_listen read/write code to process_read and
	14	process_write, reorganized file
	15	* wiimote_read - changed to obey decode flag only for register read
	16	* wiimote_connect - changed memcpy to bacmp
7	17
8	18	2007-03-08 L. Donnie Smith <cwiid@abstrakraft.org>

wiimote/command.c

ra339960	r00aa494
16	16	*
17	17	* ChangeLog:
	18	* 03/14/2007: L. Donnie Smith <cwiid@abstrakraft.org>
	19	* * audited error checking (coda and error handler sections)
	20	* * updated comments
	21	*
18	22	* 03/06/2007: L. Donnie Smith <cwiid@abstrakraft.org>
19	23	* * added wiimote parameter to wiimote_err calls
…	…
106	110	struct write_seq *ir_enable_seq;
107	111	int seq_len;
	112	int ret = 0;
108	113
109	114	/* Lock wiimote access */
110	115	if (pthread_mutex_lock(&wiimote->wiimote_mutex)) {
111	116	wiimote_err(wiimote, "Error locking rw_mutex");
112		return -1;
	117	ret = -1;
	118	goto CODA;
113	119	}
114	120
…	…
118	124	}
119	125
	126	/* Pick a report mode based on report flags */
120	127	if ((flags & WIIMOTE_RPT_EXT) &&
121	128	((wiimote->extension == WIIMOTE_EXT_NUNCHUK) \|\|
…	…
161	168	if (exec_write_seq(wiimote, seq_len, ir_enable_seq)) {
162	169	wiimote_err(wiimote, "Error on IR enable");
163		if (pthread_mutex_unlock(&wiimote->wiimote_mutex)) {
164		wiimote_err(wiimote,
165		"Error unlocking wiimote_mutex: deadlock warning");
166		}
167		return -1;
	170	ret = -1;
	171	goto CODA;
168	172	}
169	173	}
…	…
174	178	ir_disable_seq)) {
175	179	wiimote_err(wiimote, "Error on IR enable");
176		if (pthread_mutex_unlock(&wiimote->wiimote_mutex)) {
177		wiimote_err(wiimote,
178		"Error unlocking wiimote_mutex: deadlock warning");
179		}
180		return -1;
	180	ret = -1;
	181	goto CODA;
181	182	}
182	183	}
…	…
187	188	if (send_report(wiimote, 0, RPT_RPT_MODE, RPT_MODE_BUF_LEN, buf)) {
188	189	wiimote_err(wiimote, "Error setting report state");
189		if (pthread_mutex_unlock(&wiimote->wiimote_mutex)) {
190		wiimote_err(wiimote,
191		"Error unlocking wiimote_mutex: deadlock warning");
192		}
193		return -1;
	190	ret = -1;
	191	goto CODA;
194	192	}
195	193
196	194	wiimote->rpt_mode_flags = flags;
197	195
	196	CODA:
198	197	/* Unlock wiimote_mutex */
199	198	if (pthread_mutex_unlock(&wiimote->wiimote_mutex)) {
…	…
202	201	}
203	202
204		return 0;
	203	return ret;
205	204	}
206	205

wiimote/connect.c

ra339960	r00aa494
16	16	*
17	17	* ChangeLog:
	18	* 03/14/2007: L. Donnie Smith <cwiid@abstrakraft.org>
	19	* * changed memcpy to bacmp
	20	* * audited error checking (coda and error handler sections)
	21	* * updated comments
	22	*
18	23	* 03/06/2007: L. Donnie Smith <cwiid@abstrakraft.org>
19	24	* * added wiimote parameter to wiimote_err calls
…	…
40	45	wiimote_mesg_callback_t mesg_callback, int id)
41	46	{
42		struct wiimote *wiimote;
	47	struct wiimote *wiimote = NULL;
43	48	struct sockaddr_l2 ctl_remote_addr, int_remote_addr;
44	49
45		/* TODO: clean up error checking and backout code */
46
47		/* TODO: first two wiimote_err calls may pass bad ids due to race
48		* conditions. If this is acceptable, at least document it. */
49
50		/* Verify valid wiimote state */
	50	/* Allocate wiimote */
51	51	if ((wiimote = malloc(sizeof *wiimote)) == NULL) {
52		wiimote_err(NULL, "Error allocate wiimote)");
53		return NULL;
54		}
	52	wiimote_err(NULL, "Error allocating wiimote");
	53	goto ERR_HND;
	54	}
	55
	56	/* Set wiimote members for proper error detection */
	57	wiimote->ctl_socket = -1;
	58	wiimote->int_socket = -1;
	59	wiimote->dispatch_queue = NULL;
55	60
56	61	/* Global Lock, Store and Increment wiimote_id */
57	62	if (pthread_mutex_lock(&global_mutex)) {
58	63	wiimote_err(NULL, "Error locking global lock");
59		free(wiimote);
60		return NULL;
	64	goto ERR_HND;
61	65	}
62	66	wiimote->id = wiimote_id++;
63	67	if (pthread_mutex_unlock(&global_mutex)) {
64	68	wiimote_err(wiimote, "Error unlocking global lock");
65		~~free(wiimote)~~;
66		~~return NULL;~~
67		}
	69	goto ERR_HND;
	70	}
	71	/* Return the id in a pointer, if desired */
68	72	if (id) {
69	73	*id = wiimote->id;
…	…
73	77	wiimote->mesg_callback = mesg_callback;
74	78
75		/* Get bdaddr */
76		/* TODO: better way to compare ? */
77		if (memcmp(&bdaddr, BDADDR_ANY, sizeof(bdaddr_t)) == 0) {
	79	/* If BDADDR_ANY is given, find available wiimote */
	80	if (bacmp(&bdaddr, BDADDR_ANY) == 0) {
78	81	if (wiimote_findfirst(&bdaddr)) {
	82	/* TODO: wiimote functions should print their own errors */
79	83	wiimote_err(wiimote, "Unable to find wiimote");
80		free(wiimote);
81		return NULL;
	84	goto ERR_HND;
82	85	}
83	86	}
…	…
94	97	int_remote_addr.l2_psm = htobs(INT_PSM);
95	98
96		/* Get sockets */
97		wiimote->ctl_socket = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
98		wiimote->int_socket = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
99
100		/* Connect */
	99	/* Get Bluetooth Sockets */
	100	if ((wiimote->ctl_socket =
	101	socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
	102	wiimote_err(wiimote, "Error opening control socket");
	103	goto ERR_HND;
	104	}
	105	if ((wiimote->int_socket =
	106	socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
	107	wiimote_err(wiimote, "Error opening interrupt socket");
	108	goto ERR_HND;
	109	}
	110
	111	/* Connect to Wiimote */
101	112	if (connect(wiimote->ctl_socket, (struct sockaddr *)&ctl_remote_addr,
102	113	sizeof(ctl_remote_addr))) {
103		wiimote_err(wiimote,
104		"Error establishing control channel connection");
105		free(wiimote);
106		return NULL;
107		}
108		else if (connect(wiimote->int_socket, (struct sockaddr *)&int_remote_addr,
109		sizeof(int_remote_addr))) {
110		close(wiimote->ctl_socket);
111		wiimote_err(wiimote,
112		"Error establishing interrupt channel connection");
113		free(wiimote);
114		return NULL;
115		}
116
	114	wiimote_err(wiimote, "Error opening control channel");
	115	goto ERR_HND;
	116	}
	117	if (connect(wiimote->int_socket, (struct sockaddr *)&int_remote_addr,
	118	sizeof(int_remote_addr))) {
	119	wiimote_err(wiimote, "Error opening interrupt channel");
	120	goto ERR_HND;
	121	}
	122
	123	/* Create Dispatch Queue */
117	124	if ((wiimote->dispatch_queue = queue_new()) == NULL) {
118		~~close(wiimote->int_socket);~~
119		~~close(wiimote->ctl_socket);~~
120	125	wiimote_err(wiimote, "Error creating dispatch queue");
121		free(wiimote);
122		}
123
124		/* TODO: this should have finer grained error checking and
125		* backout logic (pthread__destroy) /
	126	goto ERR_HND;
	127	}
	128
	129	/* TODO: backout logic (pthread__destroy) /
126	130	/* Mutex and cond init */
127	131	if (pthread_mutex_init(&wiimote->wiimote_mutex, NULL) \|\|
…	…
129	133	pthread_cond_init(&wiimote->rw_cond, NULL) \|\|
130	134	pthread_mutex_init(&wiimote->rw_cond_mutex, NULL)) {
131		~~close(wiimote->int_socket);~~
132		~~close(wiimote->ctl_socket);~~
133		~~queue_free(wiimote->dispatch_queue, (free_func_t *)free_mesg_array);~~
134	135	wiimote_err(wiimote,
135	136	"Error initializing synchronization variables");
136		free(wiimote);
137		return NULL;
138		}
	137	goto ERR_HND;
	138	}
	139
	140	/* Set rw_status before interrupt thread */
139	141	wiimote->rw_status = RW_NONE;
140	142
…	…
142	144	if (pthread_create(&wiimote->int_listen_thread, NULL,
143	145	(void ()(void *))&int_listen, wiimote)) {
144		~~close(wiimote->int_socket);~~
145		~~close(wiimote->ctl_socket);~~
146		~~queue_free(wiimote->dispatch_queue, (free_func_t *)free_mesg_array);~~
147	146	wiimote_err(wiimote,
148	147	"Error creating interrupt channel listener thread");
149		free(wiimote);
150		return NULL;
	148	goto ERR_HND;
151	149	}
152	150	if (pthread_create(&wiimote->dispatch_thread, NULL,
153	151	(void ()(void *))&dispatch, wiimote)) {
154		~~close(wiimote->int_socket);~~
155		~~close(wiimote->ctl_socket);~~
156	152	pthread_cancel(wiimote->int_listen_thread);
157	153	pthread_join(wiimote->int_listen_thread, NULL);
158		~~queue_free(wiimote->dispatch_queue, (free_func_t *)free_mesg_array);~~
159	154	wiimote_err(wiimote, "Error creating dispatch thread");
160		free(wiimote);
161		return NULL;
	155	goto ERR_HND;
162	156	}
163	157
…	…
171	165
172	166	return wiimote;
	167
	168	ERR_HND:
	169	if (wiimote) {
	170	if (wiimote->dispatch_queue) {
	171	queue_free(wiimote->dispatch_queue,
	172	(free_func_t *)free_mesg_array);
	173	}
	174	if (wiimote->int_socket != -1) {
	175	if (close(wiimote->int_socket)) {
	176	wiimote_err(wiimote, "Error closing interrupt channel");
	177	}
	178	}
	179	if (wiimote->ctl_socket != -1) {
	180	if (close(wiimote->ctl_socket)) {
	181	wiimote_err(wiimote, "Error closing control channel");
	182	}
	183	}
	184	free(wiimote);
	185	}
	186	return NULL;
173	187	}
174	188

wiimote/event.c

ra339960	r00aa494
16	16	*
17	17	* ChangeLog:
	18	* 03/14/2007: L. Donnie Smith <cwiid@abstrakraft.org>
	19	* * audit error checking
	20	* * reorganized file
	21	* * moved int_listen read/write code to process_read and process_write
	22	* * updated (some/a few) comments
	23	*
18	24	* 03/06/2007: L. Donnie Smith <cwiid@abstrakraft.org>
19	25	* * added wiimote parameter to wiimote_err calls
…	…
92	98	#include "queue.h"
93	99
94		static int process_status(struct wiimote wiimote, const unsigned char data,
95		struct mesg_array *mesg_array)
96		{
97		struct wiimote_status_mesg *mesg;
98
99		if ((mesg = malloc(sizeof *mesg)) == NULL) {
100		wiimote_err(wiimote, "Error allocating status mesg");
101		}
102		mesg->type = WIIMOTE_MESG_STATUS;
103		mesg->battery = data[5];
104		if (data[2] & 0x02) {
105		/* dispatch will figure out what it is */
106		mesg->extension = WIIMOTE_EXT_UNKNOWN;
107		}
108		else {
109		mesg->extension = WIIMOTE_EXT_NONE;
110		}
111
112		mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
113		mesg_array->count++;
114
115		return 0;
116		}
117
118		static int process_btn(struct wiimote wiimote, const unsigned char data,
119		struct mesg_array *mesg_array)
120		{
121		struct wiimote_btn_mesg *mesg;
122		uint16_t buttons;
123
124		buttons = (data[0] & BTN_MASK_0)<<8 \|
125		(data[1] & BTN_MASK_1);
126		if (wiimote->buttons != buttons) {
127		wiimote->buttons = buttons;
128
129		if (wiimote->rpt_mode_flags & WIIMOTE_RPT_BTN) {
130		if ((mesg = malloc(sizeof *mesg)) == NULL) {
131		wiimote_err(wiimote, "Error allocating btn mesg");
132		return -1;
133		}
134		mesg->type = WIIMOTE_MESG_BTN;
135		mesg->buttons = buttons;
136
137		mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
138		mesg_array->count++;
139		}
140		}
141
142		return 0;
143		}
144
145		static int process_acc(struct wiimote wiimote, const unsigned char data,
146		struct mesg_array *mesg_array)
147		{
148		struct wiimote_acc_mesg *mesg;
149
150		if (wiimote->rpt_mode_flags & WIIMOTE_RPT_ACC) {
151		if ((mesg = malloc(sizeof *mesg)) == NULL) {
152		wiimote_err(wiimote, "Error allocating acc mesg");
153		return -1;
154		}
155		mesg->type = WIIMOTE_MESG_ACC;
156		mesg->x = data[0];
157		mesg->y = data[1];
158		mesg->z = data[2];
159
160		mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
161		mesg_array->count++;
162		}
163
164		return 0;
165		}
166
167		static int process_ir10(struct wiimote wiimote, const unsigned char data,
168		struct mesg_array *mesg_array)
169		{
170		struct wiimote_ir_mesg *mesg;
171		int i;
172		const unsigned char *block;
173
174		if (wiimote->rpt_mode_flags & WIIMOTE_RPT_IR) {
175		if ((mesg = malloc(sizeof *mesg)) == NULL) {
176		wiimote_err(wiimote, "Error allocating ir mesg");
177		return -1;
178		}
179		mesg->type = WIIMOTE_MESG_IR;
180
181		for (i=0, block=data; i < WIIMOTE_IR_SRC_COUNT; i+=2, block+=5) {
182		if (block[0] == 0xFF) {
183		mesg->src[i].valid = 0;
184		}
185		else {
186		mesg->src[i].valid = 1;
187		mesg->src[i].x = ((uint16_t)block[2] & 0x30)<<4 \|
188		(uint16_t)block[0];
189		mesg->src[i].y = ((uint16_t)block[2] & 0xC0)<<2 \|
190		(uint16_t)block[1];
191		mesg->src[i].size = -1;
192		}
193
194		if (block[3] == 0xFF) {
195		mesg->src[i+1].valid = 0;
196		}
197		else {
198		mesg->src[i+1].valid = 1;
199		mesg->src[i+1].x = ((uint16_t)block[2] & 0x03)<<8 \|
200		(uint16_t)block[3];
201		mesg->src[i+1].y = ((uint16_t)block[2] & 0x0C)<<6 \|
202		(uint16_t)block[4];
203		mesg->src[i+1].size = -1;
204		}
205		}
206
207		mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
208		mesg_array->count++;
209		}
210
211		return 0;
212		}
213
214		static int process_ir12(struct wiimote wiimote, const unsigned char data,
215		struct mesg_array *mesg_array)
216		{
217		struct wiimote_ir_mesg *mesg;
218		int i;
219		const unsigned char *block;
220
221		if (wiimote->rpt_mode_flags & WIIMOTE_RPT_IR) {
222		if ((mesg = malloc(sizeof *mesg)) == NULL) {
223		wiimote_err(wiimote, "Error allocating ir mesg");
224		return -1;
225		}
226		mesg->type = WIIMOTE_MESG_IR;
227
228		for (i=0, block=data; i < WIIMOTE_IR_SRC_COUNT; i++, block+=3) {
229		if (block[0] == 0xFF) {
230		mesg->src[i].valid = 0;
231		}
232		else {
233		mesg->src[i].valid = 1;
234		mesg->src[i].x = ((uint16_t)block[2] & 0x30)<<4 \|
235		(uint16_t)block[0];
236		mesg->src[i].y = ((uint16_t)block[2] & 0xC0)<<2 \|
237		(uint16_t)block[1];
238		mesg->src[i].size = block[2] & 0x0F;
239		}
240		}
241
242		mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
243		mesg_array->count++;
244		}
245
246		return 0;
247		}
248
249		static int process_ext(struct wiimote wiimote, unsigned char data,
250		unsigned char len, struct mesg_array *mesg_array)
251		{
252		struct wiimote_nunchuk_mesg *nunchuk_mesg;
253		struct wiimote_classic_mesg *classic_mesg;
254		int i;
255
256		switch (wiimote->extension) {
257		case WIIMOTE_EXT_NONE:
258		wiimote_err(wiimote,
259		"Extension report received with no extension present");
260		break;
261		case WIIMOTE_EXT_UNKNOWN:
262		break;
263		case WIIMOTE_EXT_NUNCHUK:
264		if (wiimote->rpt_mode_flags & WIIMOTE_RPT_NUNCHUK) {
265		if ((nunchuk_mesg = malloc(sizeof *nunchuk_mesg)) == NULL) {
266		wiimote_err(wiimote, "Error allocating nunchuk mesg");
267		return -1;
268		}
269
270		nunchuk_mesg->type = WIIMOTE_MESG_NUNCHUK;
271		nunchuk_mesg->stick_x = DECODE(data[0]);
272		nunchuk_mesg->stick_y = DECODE(data[1]);
273		nunchuk_mesg->acc_x = DECODE(data[2]);
274		nunchuk_mesg->acc_y = DECODE(data[3]);
275		nunchuk_mesg->acc_z = DECODE(data[4]);
276		nunchuk_mesg->buttons = ~DECODE(data[5]) & NUNCHUK_BTN_MASK;
277
278		mesg_array->mesg[mesg_array->count] =
279		(union wiimote_mesg *)nunchuk_mesg;
280		mesg_array->count++;
281		}
282		break;
283		case WIIMOTE_EXT_CLASSIC:
284		if (wiimote->rpt_mode_flags & WIIMOTE_RPT_CLASSIC) {
285		if ((classic_mesg = malloc(sizeof *classic_mesg)) == NULL) {
286		wiimote_err(wiimote, "Error allocating classic mesg");
287		return -1;
288		}
289
290		for (i=0; i < 6; i++) {
291		data[i] = DECODE(data[i]);
292		}
293
294		classic_mesg->type = WIIMOTE_MESG_CLASSIC;
295		classic_mesg->l_stick_x = data[0] & 0x3F;
296		classic_mesg->l_stick_y = data[1] & 0x3F;
297		classic_mesg->r_stick_x = (data[0] & 0xC0)>>3 \|
298		(data[1] & 0xC0)>>5 \|
299		(data[2] & 0x80)>>7;
300		classic_mesg->r_stick_y = data[2] & 0x1F;
301		classic_mesg->l = (data[2] & 0x60)>>2 \|
302		(data[3] & 0xE0)>>5;
303		classic_mesg->r = data[3] & 0x1F;
304		classic_mesg->buttons = ~((uint16_t)data[4]<<8 \|
305		(uint16_t)data[5]);
306
307		mesg_array->mesg[mesg_array->count] =
308		(union wiimote_mesg *)classic_mesg;
309		mesg_array->count++;
310		}
311		break;
312		}
313
314		return 0;
315		}
	100	/* process_* messages (except read and write ) allocate and fill in
	101	* wiimote_*_mesg structs from raw wiimote data. Messages are then
	102	* placed at the end of mesg_array. */
	103
	104	static int process_status(struct wiimote , const unsigned char ,
	105	struct mesg_array *);
	106	static int process_btn(struct wiimote , const unsigned char ,
	107	struct mesg_array *);
	108	static int process_acc(struct wiimote , const unsigned char ,
	109	struct mesg_array *);
	110	static int process_ir10(struct wiimote , const unsigned char ,
	111	struct mesg_array *);
	112	static int process_ir12(struct wiimote , const unsigned char ,
	113	struct mesg_array *);
	114	static int process_ext(struct wiimote , unsigned char , unsigned char,
	115	struct mesg_array *);
	116
	117	/* process_read and process_write handle copying data between wiimote buffers
	118	* and r/w buffers, as well as communication between wiimote_read or
	119	* wiimote_write */
	120	static int process_read(struct wiimote , unsigned char );
	121	static int process_write(struct wiimote *);
316	122
317	123	#define READ_BUF_LEN 23
…	…
327	133	if ((len = read(wiimote->int_socket, buf, READ_BUF_LEN)) == -1) {
328	134	wiimote_err(wiimote, "Interrupt channel read error");
	135	/* TODO: return ? */
329	136	}
330	137	else {
…	…
403	210	break;
404	211	case RPT_BTN_ACC_IR10_EXT6:
405		process_btn(wiimote, &buf[2], mesg_array);
406		process_acc(wiimote, &buf[4], mesg_array);
407		process_ir10(wiimote, &buf[7], mesg_array);
408		process_ext(wiimote, &buf[17], 6, mesg_array);
	212	if (process_btn(wiimote, &buf[2], mesg_array) \|\|
	213	process_acc(wiimote, &buf[4], mesg_array) \|\|
	214	process_ir10(wiimote, &buf[7], mesg_array) \|\|
	215	process_ext(wiimote, &buf[17], 6, mesg_array)) {
	216	err = -1;
	217	}
409	218	break;
410	219	case RPT_EXT21:
…	…
432	241	break;
433	242	case RPT_READ_DATA:
434		if (wiimote->rw_status == RW_PENDING) {
435		uint8_t data_len;
436		uint8_t error;
437
438		/* Extract error status and current packet length */
439		data_len = (buf[4]>>4)+1;
440		error = buf[4] & 0x0F;
441		/* Error if wiimote errors, or if packet is too long */
442		if (((data_len+wiimote->read_received)>
443		wiimote->read_len) \|\| error) {
444		wiimote_err(wiimote, "Error in read data");
445		wiimote->rw_status = RW_ERROR;
446		}
447		else {
448		/* Copy data into read_buf, update read data, signal
449		* ready if we have enough data */
450		memcpy(wiimote->read_buf, buf+7, data_len);
451		wiimote->read_buf += data_len;
452		wiimote->read_received += data_len;
453		if (wiimote->read_received == wiimote->read_len) {
454		wiimote->rw_status = RW_READY;
455		}
456		}
457		if (wiimote->rw_status != RW_PENDING) {
458		/* Lock rw_cond_mutex, signal rw_cond, unlock
459		* rw_cond_mutex */
460		if (pthread_mutex_lock(&wiimote->rw_cond_mutex)) {
461		wiimote_err(wiimote,
462		"Error locking rw_cond_mutex");
463		}
464		else {
465		if (pthread_cond_signal(&wiimote->rw_cond)) {
466		wiimote_err(wiimote,
467		"Error signaling rw_cond: "
468		"deadlock warning");
469		}
470		if (pthread_mutex_unlock(
471		&wiimote->rw_cond_mutex)) {
472		wiimote_err(wiimote,
473		"Error unlocking rw_cond_mutex: "
474		"deadlock warning");
475		}
476		}
477		}
478		/* TODO: add button message */
479		}
480		else {
481		wiimote_err(wiimote, "Extraneous read data received");
482		}
	243	process_read(wiimote, &buf[4]);
	244	/* TODO: send button message */
483	245	break;
484	246	case RPT_WRITE_ACK:
485		if (wiimote->rw_status == RW_PENDING) {
486		wiimote->rw_status = RW_READY;
487		/* Lock write_cond_mutex, signal write_cond, unlock
488		* write_cond_mutex */
489		if (pthread_mutex_lock(&wiimote->rw_cond_mutex)) {
490		wiimote_err(wiimote,
491		"Error locking rw_cond_mutex");
492		}
493		else {
494		if (pthread_cond_signal(&wiimote->rw_cond)) {
495		wiimote_err(wiimote,
496		"Error signaling rw_cond: "
497		"deadlock warning");
498		}
499		if (pthread_mutex_unlock(
500		&wiimote->rw_cond_mutex)) {
501		wiimote_err(wiimote,
502		"Error unlocking rw_cond_mutex: "
503		"deadlock warning");
504		}
505		}
506		}
507		else {
508		wiimote_err(wiimote, "Extraneous write ack received");
509		}
	247	process_write(wiimote);
510	248	break;
511	249	default:
…	…
520	258	}
521	259
	260	static int process_status(struct wiimote wiimote, const unsigned char data,
	261	struct mesg_array *mesg_array)
	262	{
	263	struct wiimote_status_mesg *mesg;
	264
	265	if ((mesg = malloc(sizeof *mesg)) == NULL) {
	266	wiimote_err(wiimote, "Error allocating status mesg");
	267	return -1;
	268	}
	269
	270	mesg->type = WIIMOTE_MESG_STATUS;
	271	mesg->battery = data[5];
	272	if (data[2] & 0x02) {
	273	/* dispatch will figure out what it is */
	274	mesg->extension = WIIMOTE_EXT_UNKNOWN;
	275	}
	276	else {
	277	mesg->extension = WIIMOTE_EXT_NONE;
	278	}
	279
	280	mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
	281	mesg_array->count++;
	282
	283	return 0;
	284	}
	285
	286	static int process_btn(struct wiimote wiimote, const unsigned char data,
	287	struct mesg_array *mesg_array)
	288	{
	289	struct wiimote_btn_mesg *mesg;
	290	uint16_t buttons;
	291
	292	buttons = (data[0] & BTN_MASK_0)<<8 \|
	293	(data[1] & BTN_MASK_1);
	294	if (wiimote->buttons != buttons) {
	295	wiimote->buttons = buttons;
	296
	297	if (wiimote->rpt_mode_flags & WIIMOTE_RPT_BTN) {
	298	if ((mesg = malloc(sizeof *mesg)) == NULL) {
	299	wiimote_err(wiimote, "Error allocating btn mesg");
	300	return -1;
	301	}
	302	mesg->type = WIIMOTE_MESG_BTN;
	303	mesg->buttons = buttons;
	304
	305	mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
	306	mesg_array->count++;
	307	}
	308	}
	309
	310	return 0;
	311	}
	312
	313	static int process_acc(struct wiimote wiimote, const unsigned char data,
	314	struct mesg_array *mesg_array)
	315	{
	316	struct wiimote_acc_mesg *mesg;
	317
	318	if (wiimote->rpt_mode_flags & WIIMOTE_RPT_ACC) {
	319	if ((mesg = malloc(sizeof *mesg)) == NULL) {
	320	wiimote_err(wiimote, "Error allocating acc mesg");
	321	return -1;
	322	}
	323	mesg->type = WIIMOTE_MESG_ACC;
	324	mesg->x = data[0];
	325	mesg->y = data[1];
	326	mesg->z = data[2];
	327
	328	mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
	329	mesg_array->count++;
	330	}
	331
	332	return 0;
	333	}
	334
	335	static int process_ir10(struct wiimote wiimote, const unsigned char data,
	336	struct mesg_array *mesg_array)
	337	{
	338	struct wiimote_ir_mesg *mesg;
	339	int i;
	340	const unsigned char *block;
	341
	342	if (wiimote->rpt_mode_flags & WIIMOTE_RPT_IR) {
	343	if ((mesg = malloc(sizeof *mesg)) == NULL) {
	344	wiimote_err(wiimote, "Error allocating ir mesg");
	345	return -1;
	346	}
	347	mesg->type = WIIMOTE_MESG_IR;
	348
	349	for (i=0, block=data; i < WIIMOTE_IR_SRC_COUNT; i+=2, block+=5) {
	350	if (block[0] == 0xFF) {
	351	mesg->src[i].valid = 0;
	352	}
	353	else {
	354	mesg->src[i].valid = 1;
	355	mesg->src[i].x = ((uint16_t)block[2] & 0x30)<<4 \|
	356	(uint16_t)block[0];
	357	mesg->src[i].y = ((uint16_t)block[2] & 0xC0)<<2 \|
	358	(uint16_t)block[1];
	359	mesg->src[i].size = -1;
	360	}
	361
	362	if (block[3] == 0xFF) {
	363	mesg->src[i+1].valid = 0;
	364	}
	365	else {
	366	mesg->src[i+1].valid = 1;
	367	mesg->src[i+1].x = ((uint16_t)block[2] & 0x03)<<8 \|
	368	(uint16_t)block[3];
	369	mesg->src[i+1].y = ((uint16_t)block[2] & 0x0C)<<6 \|
	370	(uint16_t)block[4];
	371	mesg->src[i+1].size = -1;
	372	}
	373	}
	374
	375	mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
	376	mesg_array->count++;
	377	}
	378
	379	return 0;
	380	}
	381
	382	static int process_ir12(struct wiimote wiimote, const unsigned char data,
	383	struct mesg_array *mesg_array)
	384	{
	385	struct wiimote_ir_mesg *mesg;
	386	int i;
	387	const unsigned char *block;
	388
	389	if (wiimote->rpt_mode_flags & WIIMOTE_RPT_IR) {
	390	if ((mesg = malloc(sizeof *mesg)) == NULL) {
	391	wiimote_err(wiimote, "Error allocating ir mesg");
	392	return -1;
	393	}
	394	mesg->type = WIIMOTE_MESG_IR;
	395
	396	for (i=0, block=data; i < WIIMOTE_IR_SRC_COUNT; i++, block+=3) {
	397	if (block[0] == 0xFF) {
	398	mesg->src[i].valid = 0;
	399	}
	400	else {
	401	mesg->src[i].valid = 1;
	402	mesg->src[i].x = ((uint16_t)block[2] & 0x30)<<4 \|
	403	(uint16_t)block[0];
	404	mesg->src[i].y = ((uint16_t)block[2] & 0xC0)<<2 \|
	405	(uint16_t)block[1];
	406	mesg->src[i].size = block[2] & 0x0F;
	407	}
	408	}
	409
	410	mesg_array->mesg[mesg_array->count] = (union wiimote_mesg *)mesg;
	411	mesg_array->count++;
	412	}
	413
	414	return 0;
	415	}
	416
	417	static int process_ext(struct wiimote wiimote, unsigned char data,
	418	unsigned char len, struct mesg_array *mesg_array)
	419	{
	420	struct wiimote_nunchuk_mesg *nunchuk_mesg;
	421	struct wiimote_classic_mesg *classic_mesg;
	422	int i;
	423
	424	switch (wiimote->extension) {
	425	case WIIMOTE_EXT_NONE:
	426	wiimote_err(wiimote,
	427	"Extension report received with no extension present");
	428	break;
	429	case WIIMOTE_EXT_UNKNOWN:
	430	break;
	431	case WIIMOTE_EXT_NUNCHUK:
	432	if (wiimote->rpt_mode_flags & WIIMOTE_RPT_NUNCHUK) {
	433	if ((nunchuk_mesg = malloc(sizeof *nunchuk_mesg)) == NULL) {
	434	wiimote_err(wiimote, "Error allocating nunchuk mesg");
	435	return -1;
	436	}
	437
	438	nunchuk_mesg->type = WIIMOTE_MESG_NUNCHUK;
	439	nunchuk_mesg->stick_x = DECODE(data[0]);
	440	nunchuk_mesg->stick_y = DECODE(data[1]);
	441	nunchuk_mesg->acc_x = DECODE(data[2]);
	442	nunchuk_mesg->acc_y = DECODE(data[3]);
	443	nunchuk_mesg->acc_z = DECODE(data[4]);
	444	nunchuk_mesg->buttons = ~DECODE(data[5]) & NUNCHUK_BTN_MASK;
	445
	446	mesg_array->mesg[mesg_array->count] =
	447	(union wiimote_mesg *)nunchuk_mesg;
	448	mesg_array->count++;
	449	}
	450	break;
	451	case WIIMOTE_EXT_CLASSIC:
	452	if (wiimote->rpt_mode_flags & WIIMOTE_RPT_CLASSIC) {
	453	if ((classic_mesg = malloc(sizeof *classic_mesg)) == NULL) {
	454	wiimote_err(wiimote, "Error allocating classic mesg");
	455	return -1;
	456	}
	457
	458	for (i=0; i < 6; i++) {
	459	data[i] = DECODE(data[i]);
	460	}
	461
	462	classic_mesg->type = WIIMOTE_MESG_CLASSIC;
	463	classic_mesg->l_stick_x = data[0] & 0x3F;
	464	classic_mesg->l_stick_y = data[1] & 0x3F;
	465	classic_mesg->r_stick_x = (data[0] & 0xC0)>>3 \|
	466	(data[1] & 0xC0)>>5 \|
	467	(data[2] & 0x80)>>7;
	468	classic_mesg->r_stick_y = data[2] & 0x1F;
	469	classic_mesg->l = (data[2] & 0x60)>>2 \|
	470	(data[3] & 0xE0)>>5;
	471	classic_mesg->r = data[3] & 0x1F;
	472	classic_mesg->buttons = ~((uint16_t)data[4]<<8 \|
	473	(uint16_t)data[5]);
	474
	475	mesg_array->mesg[mesg_array->count] =
	476	(union wiimote_mesg *)classic_mesg;
	477	mesg_array->count++;
	478	}
	479	break;
	480	}
	481
	482	return 0;
	483	}
	484
	485	static int process_read(struct wiimote wiimote, unsigned char data)
	486	{
	487	uint8_t data_len;
	488	uint8_t error;
	489	int ret = 0;
	490
	491	if (wiimote->rw_status == RW_PENDING) {
	492	/* Extract error status and current packet length */
	493	data_len = (data[0]>>4)+1;
	494	error = data[0] & 0x0F;
	495	/* Error if wiimote errors, or if packet is too long */
	496	if (((data_len + wiimote->read_received) > wiimote->read_len) \|\|
	497	error) {
	498	wiimote_err(wiimote, "Error in read data");
	499	wiimote->rw_status = RW_ERROR;
	500	ret = -1;
	501	}
	502	else {
	503	/* Copy data into read_buf, update read data, signal
	504	* ready if we have enough data */
	505	memcpy(wiimote->read_buf, data+3, data_len);
	506	wiimote->read_buf += data_len;
	507	wiimote->read_received += data_len;
	508	if (wiimote->read_received == wiimote->read_len) {
	509	wiimote->rw_status = RW_READY;
	510	}
	511	}
	512	if (wiimote->rw_status != RW_PENDING) {
	513	/* Lock rw_cond_mutex, signal rw_cond, unlock
	514	* rw_cond_mutex */
	515	if (pthread_mutex_lock(&wiimote->rw_cond_mutex)) {
	516	wiimote_err(wiimote, "Error locking rw_cond_mutex");
	517	ret = -1;
	518	}
	519	else {
	520	if (pthread_cond_signal(&wiimote->rw_cond)) {
	521	wiimote_err(wiimote, "Error signaling rw_cond: "
	522	"deadlock warning");
	523	ret = -1;
	524	}
	525	if (pthread_mutex_unlock(
	526	&wiimote->rw_cond_mutex)) {
	527	wiimote_err(wiimote, "Error unlocking rw_cond_mutex: "
	528	"deadlock warning");
	529	ret = -1;
	530	}
	531	}
	532	}
	533	}
	534	else {
	535	wiimote_err(wiimote, "Extraneous read data received");
	536	ret = 1;
	537	}
	538
	539	return ret;
	540	}
	541
	542	static int process_write(struct wiimote *wiimote)
	543	{
	544	int ret = 0;
	545
	546	if (wiimote->rw_status == RW_PENDING) {
	547	wiimote->rw_status = RW_READY;
	548	/* Lock write_cond_mutex, signal write_cond, unlock
	549	* write_cond_mutex */
	550	if (pthread_mutex_lock(&wiimote->rw_cond_mutex)) {
	551	wiimote_err(wiimote, "Error locking rw_cond_mutex");
	552	ret = -1;
	553	}
	554	else {
	555	if (pthread_cond_signal(&wiimote->rw_cond)) {
	556	wiimote_err(wiimote, "Error signaling rw_cond: "
	557	"deadlock warning");
	558	ret = -1;
	559	}
	560	if (pthread_mutex_unlock(&wiimote->rw_cond_mutex)) {
	561	wiimote_err(wiimote, "Error unlocking rw_cond_mutex: "
	562	"deadlock warning");
	563	ret = -1;
	564	}
	565	}
	566	}
	567	else {
	568	wiimote_err(wiimote, "Extraneous write ack received");
	569	ret = -1;
	570	}
	571
	572	return ret;
	573	}
	574
522	575	/* cleanup func */
523	576	static void free_dispatch_queue(struct queue *queue)
524	577	{
525	578	if (queue_free(queue, (free_func_t *)free_mesg_array)) {
526		/* TODO: return proper wiimote id */
	579	/* TODO: return proper wiimote ptr */
527	580	wiimote_err(NULL, "Error freeing dispatch queue");
528	581	}
…	…
549	602	while (!queue_dequeue(dispatch_queue, (void **)&mesg_array)) {
550	603	pthread_testcancel();
	604	/* Disable cancelling while in callback */
551	605	if (pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &cancelstate)) {
552	606	wiimote_err(wiimote, "Error disabling cancel state");
…	…
561	615	(wiimote->extension == WIIMOTE_EXT_NONE)) {
562	616	buf = 0x00;
	617	/* Initialize extension register space */
563	618	if (wiimote_write(wiimote, WIIMOTE_RW_REG, 0xA40040, 1,
564	619	&buf)) {
…	…
567	622	wiimote->extension = WIIMOTE_EXT_UNKNOWN;
568	623	}
	624	/* Read extension ID */
569	625	else if (wiimote_read(wiimote,
570	626	WIIMOTE_RW_REG \| WIIMOTE_RW_DECODE, 0xA400FE,
…	…
600	656	}
601	657
	658	/* Invoke Callback (for Status Messages) */
602	659	if (wiimote->rpt_mode_flags & WIIMOTE_RPT_STATUS) {
603	660	mesg->status_mesg.extension = wiimote->extension;
…	…
610	667	}
611	668	else {
	669	/* Invoke Callback (all but Status Messages) */
612	670	if (wiimote->mesg_callback) {
613	671	wiimote->mesg_callback(wiimote->id, mesg_array->count,
…	…
615	673	}
616	674	}
	675	/* Reenable Thread Cancel */
617	676	if (pthread_setcancelstate(cancelstate, &cancelstate)) {
618	677	wiimote_err(wiimote, "Error enabling cancel state");
…	…
626	685	wiimote_err(wiimote, "Exiting dispatch thread");
627	686
	687	/* Just in case, free the dispatch queue */
628	688	pthread_cleanup_pop(1);
629	689

wiimote/queue.c

rc033b7a	r00aa494
16	16	*
17	17	* ChangeLog:
	18	* 03/14/2007: L. Donnie Smith <cwiid@abstrakraft.org>
	19	* * audited error checking (coda and error handler sections)
	20	*
18	21	* 03/03/2007: L. Donnie Smith <cwiid@abstrakraft.org>
19	22	* * Initial ChangeLog
…	…
101	104	if (pthread_mutex_lock(&queue->mutex)) {
102	105	free(node);
103		return -1;
	106	ret = -1;
	107	goto CODA;
104	108	}
105	109	*queue->p_tail = node;
106	110	queue->p_tail = &node->next;
107	111	if (pthread_mutex_unlock(&queue->mutex)) {
108		return -1;
	112	ret = -1;
	113	goto CODA;
109	114	}
110	115
111	116	/* Signal dispatch condition */
112	117	if (pthread_mutex_lock(&queue->cond_mutex)) {
113		return -1;
	118	ret = -1;
	119	goto CODA;
114	120	}
115	121	if (pthread_cond_signal(&queue->cond)) {
116	122	ret = -1;
	123	goto CODA;
117	124	}
118	125	if (pthread_mutex_unlock(&queue->cond_mutex)) {
119		return -1;
	126	ret = -1;
	127	goto CODA;
120	128	}
	129
	130	CODA:
121	131	if (pthread_setcanceltype(canceltype, &canceltype)) {
122	132	ret = -1;

wiimote/rw.c

ra339960	r00aa494
16	16	*
17	17	* ChangeLog:
	18	* 03/14/2007: L. Donnie Smith <cwiid@abstrakraft.org>
	19	* * wiimote_read - changed to obey decode flag only for register read
	20	*
18	21	* 03/06/2007: L. Donnie Smith <cwiid@abstrakraft.org>
19	22	* * added wiimote parameter to wiimote_err calls
…	…
120	123	}
121	124
122		if (flags & WIIMOTE_RW_DECODE) {
	125	/* Decode only for registers */
	126	if ((flags & WIIMOTE_RW_DECODE) && (flags & WIIMOTE_RW_REG)) {
123	127	for (i=0; i < len; i++) {
124	128	((unsigned char )data)[i] = DECODE(((unsigned char )data)[i]);

wiimote/util.c

ra339960	r00aa494
16	16	*
17	17	* ChangeLog:
	18	* 03/14/2007: L. Donnie Smith <cwiid@abstrakraft.org>
	19	* * audited error checking (coda and error handler sections)
	20	*
18	21	* 03/05/2007: L. Donnie Smith <cwiid@abstrakraft.org>
19	22	* * created wiimote_err_func variable
…	…
164	167	*/
165	168	int dev_id;
166		int sock;
	169	int sock = -1;
167	170	inquiry_info *dev_list = NULL;
168	171	int i;
…	…
174	177	if ((dev_id = hci_get_route(NULL)) == -1) {
175	178	wiimote_err(NULL, "No Bluetooth device found");
176		return -1;
	179	ret = -1;
	180	goto CODA;
177	181	}
178	182	if ((sock = hci_open_dev(dev_id)) == -1) {
179	183	wiimote_err(NULL, "Error opening Bluetooth device");
180		return -1;
	184	ret = -1;
	185	goto CODA;
181	186	}
182	187
…	…
185	190	IREQ_CACHE_FLUSH)) == -1) {
186	191	wiimote_err(NULL, "Error on device inquiry");
187		~~hci_close_dev(sock)~~;
188		~~return -1~~;
	192	ret = -1;
	193	goto CODA;
189	194	}
190	195
…	…
206	211	}
207	212
208		hci_close_dev(sock);
	213	CODA:
	214	if (sock != -1) {
	215	hci_close_dev(sock);
	216	}
209	217	if (dev_list) {
210	218	free(dev_list);