root/libcwiid/connect.c

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

Basic support for cwiid_listen
  • 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 <stdlib.h>
20#include <string.h>
21#include <fcntl.h>
22#include <pthread.h>
23#include <sys/types.h>
24#include <sys/socket.h>
25#include <unistd.h>
26#include <bluetooth/bluetooth.h>
27#include <bluetooth/l2cap.h>
28#include "cwiid_internal.h"
29
30pthread_mutex_t global_mutex = PTHREAD_MUTEX_INITIALIZER;
31static int wiimote_id = 0;
32
33/* TODO: Turn this onto a macro on next major so version */
34cwiid_wiimote_t *cwiid_open(bdaddr_t *bdaddr, int flags)
35{
36        return cwiid_open_timeout(bdaddr, flags, DEFAULT_TIMEOUT);
37}
38
39cwiid_wiimote_t *cwiid_open_timeout(bdaddr_t *bdaddr, int flags, int timeout)
40{
41        struct sockaddr_l2 remote_addr;
42        int ctl_socket = -1, int_socket = -1;
43        struct wiimote *wiimote = NULL;
44
45        /* If BDADDR_ANY is given, find available wiimote */
46        if (bacmp(bdaddr, BDADDR_ANY) == 0) {
47                if (cwiid_find_wiimote(bdaddr, timeout)) {
48                        goto ERR_HND;
49                }
50                sleep(1);
51        }
52
53        /* Connect to Wiimote */
54        /* Control Channel */
55        memset(&remote_addr, 0, sizeof remote_addr);
56        remote_addr.l2_family = AF_BLUETOOTH;
57        remote_addr.l2_bdaddr = *bdaddr;
58        remote_addr.l2_psm = htobs(CTL_PSM);
59        if ((ctl_socket =
60          socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
61                cwiid_err(NULL, "Socket creation error (control socket)");
62                goto ERR_HND;
63        }
64        if (connect(ctl_socket, (struct sockaddr *)&remote_addr,
65                        sizeof remote_addr)) {
66                cwiid_err(NULL, "Socket connect error (control socket)");
67                goto ERR_HND;
68        }
69
70        /* Interrupt Channel */
71        remote_addr.l2_psm = htobs(INT_PSM);
72        if ((int_socket =
73          socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
74                cwiid_err(NULL, "Socket creation error (interrupt socket)");
75                goto ERR_HND;
76        }
77        if (connect(int_socket, (struct sockaddr *)&remote_addr,
78                        sizeof remote_addr)) {
79                cwiid_err(NULL, "Socket connect error (interrupt socket)");
80                goto ERR_HND;
81        }
82
83        if ((wiimote = cwiid_new(ctl_socket, int_socket, flags)) == NULL) {
84                /* Raises its own error */
85                goto ERR_HND;
86        }
87
88        return wiimote;
89
90ERR_HND:
91        /* Close Sockets */
92        if (ctl_socket != -1) {
93                if (close(ctl_socket)) {
94                        cwiid_err(NULL, "Socket close error (control socket)");
95                }
96        }
97        if (int_socket != -1) {
98                if (close(int_socket)) {
99                        cwiid_err(NULL, "Socket close error (interrupt socket)");
100                }
101        }
102        return NULL;
103}
104
105cwiid_wiimote_t *cwiid_listen(int flags)
106{
107        struct sockaddr_l2 local_addr;
108        struct sockaddr_l2 remote_addr;
109        socklen_t socklen;
110        int ctl_server_socket = -1, int_server_socket = -1,
111            ctl_socket = -1, int_socket = -1;
112        struct wiimote *wiimote = NULL;
113
114        /* Connect to Wiimote */
115        /* Control Channel */
116        memset(&local_addr, 0, sizeof local_addr);
117        local_addr.l2_family = AF_BLUETOOTH;
118        local_addr.l2_bdaddr = *BDADDR_ANY;
119        local_addr.l2_psm = htobs(CTL_PSM);
120        if ((ctl_server_socket =
121          socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
122                cwiid_err(NULL, "Socket creation error (control socket)");
123                goto ERR_HND;
124        }
125        if (bind(ctl_server_socket, (struct sockaddr *)&local_addr,
126                 sizeof local_addr)) {
127                cwiid_err(NULL, "Socket bind error (control socket)");
128                goto ERR_HND;
129        }
130        if (listen(ctl_server_socket, 1)) {
131                cwiid_err(NULL, "Socket listen error (control socket)");
132                goto ERR_HND;
133        }
134
135        /* Interrupt Channel */
136        local_addr.l2_psm = htobs(INT_PSM);
137        if ((int_server_socket =
138          socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) == -1) {
139                cwiid_err(NULL, "Socket creation error (interrupt socket)");
140                goto ERR_HND;
141        }
142        if (bind(int_server_socket, (struct sockaddr *)&local_addr,
143                 sizeof local_addr)) {
144                cwiid_err(NULL, "Socket bind error (interrupt socket)");
145                goto ERR_HND;
146        }
147        if (listen(int_server_socket, 1)) {
148                cwiid_err(NULL, "Socket listen error (interrupt socket)");
149                goto ERR_HND;
150        }
151
152        /* Block for Connections */
153        if ((ctl_socket = accept(ctl_server_socket, (struct sockaddr *)&remote_addr, &socklen)) < 0) {
154                cwiid_err(NULL, "Socket accept error (control socket)");
155                goto ERR_HND;
156        }
157        if ((int_socket = accept(int_server_socket, (struct sockaddr *)&remote_addr, &socklen)) < 0) {
158                cwiid_err(NULL, "Socket accept error (interrupt socket)");
159                goto ERR_HND;
160        }
161
162        /* Close server sockets */
163        if (close(ctl_server_socket)) {
164                cwiid_err(NULL, "Socket close error (control socket)");
165        }
166        if (close(int_server_socket)) {
167                cwiid_err(NULL, "Socket close error (interrupt socket)");
168        }
169
170        if ((wiimote = cwiid_new(ctl_socket, int_socket, flags)) == NULL) {
171                /* Raises its own error */
172                goto ERR_HND;
173        }
174
175        return wiimote;
176
177ERR_HND:
178        /* Close Sockets */
179        if (ctl_server_socket != -1) {
180                if (close(ctl_server_socket)) {
181                        cwiid_err(NULL, "Socket close error (control server socket)");
182                }
183        }
184        if (int_server_socket != -1) {
185                if (close(int_server_socket)) {
186                        cwiid_err(NULL, "Socket close error (interrupt server socket)");
187                }
188        }
189        if (ctl_socket != -1) {
190                if (close(ctl_socket)) {
191                        cwiid_err(NULL, "Socket close error (control socket)");
192                }
193        }
194        if (int_socket != -1) {
195                if (close(int_socket)) {
196                        cwiid_err(NULL, "Socket close error (interrupt socket)");
197                }
198        }
199
200        return NULL;
201}
202
203cwiid_wiimote_t *cwiid_new(int ctl_socket, int int_socket, int flags)
204{
205        struct wiimote *wiimote = NULL;
206        char mesg_pipe_init = 0, status_pipe_init = 0, rw_pipe_init = 0,
207             state_mutex_init = 0, rw_mutex_init = 0, rpt_mutex_init = 0,
208             router_thread_init = 0, status_thread_init = 0;
209        void *pthread_ret;
210
211        /* Allocate wiimote */
212        if ((wiimote = malloc(sizeof *wiimote)) == NULL) {
213                cwiid_err(NULL, "Memory allocation error (cwiid_wiimote_t)");
214                goto ERR_HND;
215        }
216
217        /* set sockets and flags */
218        wiimote->ctl_socket = ctl_socket;
219        wiimote->int_socket = int_socket;
220        wiimote->flags = flags;
221
222        /* Global Lock, Store and Increment wiimote_id */
223        if (pthread_mutex_lock(&global_mutex)) {
224                cwiid_err(NULL, "Mutex lock error (global mutex)");
225                goto ERR_HND;
226        }
227        wiimote->id = wiimote_id++;
228        if (pthread_mutex_unlock(&global_mutex)) {
229                cwiid_err(wiimote, "Mutex unlock error (global mutex) - "
230                                   "deadlock warning");
231                goto ERR_HND;
232        }
233
234        /* Create pipes */
235        if (pipe(wiimote->mesg_pipe)) {
236                cwiid_err(wiimote, "Pipe creation error (mesg pipe)");
237                goto ERR_HND;
238        }
239        mesg_pipe_init = 1;
240        if (pipe(wiimote->status_pipe)) {
241                cwiid_err(wiimote, "Pipe creation error (status pipe)");
242                goto ERR_HND;
243        }
244        status_pipe_init = 1;
245        if (pipe(wiimote->rw_pipe)) {
246                cwiid_err(wiimote, "Pipe creation error (rw pipe)");
247                goto ERR_HND;
248        }
249        rw_pipe_init = 1;
250
251        /* Setup blocking */
252        if (fcntl(wiimote->mesg_pipe[1], F_SETFL, O_NONBLOCK)) {
253                cwiid_err(wiimote, "File control error (mesg write pipe)");
254                goto ERR_HND;
255        }
256        if (wiimote->flags & CWIID_FLAG_NONBLOCK) {
257                if (fcntl(wiimote->mesg_pipe[0], F_SETFL, O_NONBLOCK)) {
258                        cwiid_err(wiimote, "File control error (mesg read pipe)");
259                        goto ERR_HND;
260                }
261        }
262
263        /* Init mutexes */
264        if (pthread_mutex_init(&wiimote->state_mutex, NULL)) {
265                cwiid_err(wiimote, "Mutex initialization error (state mutex)");
266                goto ERR_HND;
267        }
268        state_mutex_init = 1;
269        if (pthread_mutex_init(&wiimote->rw_mutex, NULL)) {
270                cwiid_err(wiimote, "Mutex initialization error (rw mutex)");
271                goto ERR_HND;
272        }
273        rw_mutex_init = 1;
274        if (pthread_mutex_init(&wiimote->rpt_mutex, NULL)) {
275                cwiid_err(wiimote, "Mutex initialization error (rpt mutex)");
276                goto ERR_HND;
277        }
278        rpt_mutex_init = 1;
279
280        /* Set rw_status before starting router thread */
281        wiimote->rw_status = RW_IDLE;
282
283        /* Launch interrupt socket listener and dispatch threads */
284        if (pthread_create(&wiimote->router_thread, NULL,
285                           (void *(*)(void *))&router_thread, wiimote)) {
286                cwiid_err(wiimote, "Thread creation error (router thread)");
287                goto ERR_HND;
288        }
289        router_thread_init = 1;
290        if (pthread_create(&wiimote->status_thread, NULL,
291                           (void *(*)(void *))&status_thread, wiimote)) {
292                cwiid_err(wiimote, "Thread creation error (status thread)");
293                goto ERR_HND;
294        }
295        status_thread_init = 1;
296
297        /* Success!  Update state */
298        memset(&wiimote->state, 0, sizeof wiimote->state);
299        wiimote->mesg_callback = NULL;
300        cwiid_set_led(wiimote, 0);
301        cwiid_request_status(wiimote);
302
303        return wiimote;
304
305ERR_HND:
306        if (wiimote) {
307                /* Close threads */
308                if (router_thread_init) {
309                        pthread_cancel(wiimote->router_thread);
310                        if (pthread_join(wiimote->router_thread, &pthread_ret)) {
311                                cwiid_err(wiimote, "Thread join error (router thread)");
312                        }
313                        else if (!((pthread_ret == PTHREAD_CANCELED) &&
314                                 (pthread_ret == NULL))) {
315                                cwiid_err(wiimote, "Bad return value from router thread");
316                        }
317                }
318
319                if (status_thread_init) {
320                        pthread_cancel(wiimote->status_thread);
321                        if (pthread_join(wiimote->status_thread, &pthread_ret)) {
322                                cwiid_err(wiimote, "Thread join error (status thread)");
323                        }
324                        else if (!((pthread_ret == PTHREAD_CANCELED) && (pthread_ret == NULL))) {
325                                cwiid_err(wiimote, "Bad return value from status thread");
326                        }
327                }
328
329                /* Close Pipes */
330                if (mesg_pipe_init) {
331                        if (close(wiimote->mesg_pipe[0]) || close(wiimote->mesg_pipe[1])) {
332                                cwiid_err(wiimote, "Pipe close error (mesg pipe)");
333                        }
334                }
335                if (status_pipe_init) {
336                        if (close(wiimote->status_pipe[0]) ||
337                          close(wiimote->status_pipe[1])) {
338                                cwiid_err(wiimote, "Pipe close error (status pipe)");
339                        }
340                }
341                if (rw_pipe_init) {
342                        if (close(wiimote->rw_pipe[0]) || close(wiimote->rw_pipe[1])) {
343                                cwiid_err(wiimote, "Pipe close error (rw pipe)");
344                        }
345                }
346                /* Destroy Mutexes */
347                if (state_mutex_init) {
348                        if (pthread_mutex_destroy(&wiimote->state_mutex)) {
349                                cwiid_err(wiimote, "Mutex destroy error (state mutex)");
350                        }
351                }
352                if (rw_mutex_init) {
353                        if (pthread_mutex_destroy(&wiimote->rw_mutex)) {
354                                cwiid_err(wiimote, "Mutex destroy error (rw mutex)");
355                        }
356                }
357                if (rpt_mutex_init) {
358                        if (pthread_mutex_destroy(&wiimote->rpt_mutex)) {
359                                cwiid_err(wiimote, "Mutex destroy error (rpt mutex)");
360                        }
361                }
362                free(wiimote);
363        }
364        return NULL;
365}
366
367int cwiid_close(cwiid_wiimote_t *wiimote)
368{
369        void *pthread_ret;
370
371        /* Stop rumbling, otherwise wiimote continues to rumble for
372           few seconds after closing the connection! There should be no
373           need to check if stopping fails: we are closing the connection
374           in any case. */
375        if (wiimote->state.rumble) {
376                cwiid_set_rumble(wiimote, 0);
377        }
378
379        /* Cancel and join router_thread and status_thread */
380        if (pthread_cancel(wiimote->router_thread)) {
381                /* if thread quit abnormally, would have printed it's own error */
382        }
383        if (pthread_join(wiimote->router_thread, &pthread_ret)) {
384                cwiid_err(wiimote, "Thread join error (router thread)");
385        }
386        else if (!((pthread_ret == PTHREAD_CANCELED) || (pthread_ret == NULL))) {
387                cwiid_err(wiimote, "Bad return value from router thread");
388        }
389
390        if (pthread_cancel(wiimote->status_thread)) {
391                /* if thread quit abnormally, would have printed it's own error */
392        }
393        if (pthread_join(wiimote->status_thread, &pthread_ret)) {
394                cwiid_err(wiimote, "Thread join error (status thread)");
395        }
396        else if (!((pthread_ret == PTHREAD_CANCELED) || (pthread_ret == NULL))) {
397                cwiid_err(wiimote, "Bad return value from status thread");
398        }
399
400        if (wiimote->mesg_callback) {
401                if (cancel_mesg_callback(wiimote)) {
402                        /* prints it's own errors */
403                }
404        }
405
406        if (cancel_rw(wiimote)) {
407                /* prints it's own errors */
408        }
409
410        /* Close sockets */
411        if (close(wiimote->int_socket)) {
412                cwiid_err(wiimote, "Socket close error (interrupt socket)");
413        }
414        if (close(wiimote->ctl_socket)) {
415                cwiid_err(wiimote, "Socket close error (control socket)");
416        }
417        /* Close Pipes */
418        if (close(wiimote->mesg_pipe[0]) || close(wiimote->mesg_pipe[1])) {
419                cwiid_err(wiimote, "Pipe close error (mesg pipe)");
420        }
421        if (close(wiimote->status_pipe[0]) || close(wiimote->status_pipe[1])) {
422                cwiid_err(wiimote, "Pipe close error (status pipe)");
423        }
424        if (close(wiimote->rw_pipe[0]) || close(wiimote->rw_pipe[1])) {
425                cwiid_err(wiimote, "Pipe close error (rw pipe)");
426        }
427        /* Destroy mutexes */
428        if (pthread_mutex_destroy(&wiimote->state_mutex)) {
429                cwiid_err(wiimote, "Mutex destroy error (state)");
430        }
431        if (pthread_mutex_destroy(&wiimote->rw_mutex)) {
432                cwiid_err(wiimote, "Mutex destroy error (rw)");
433        }
434        if (pthread_mutex_destroy(&wiimote->rpt_mutex)) {
435                cwiid_err(wiimote, "Mutex destroy error (rpt)");
436        }
437
438        free(wiimote);
439
440        return 0;
441}
Note: See TracBrowser for help on using the browser.