small log issue from bug 0016664
[asterisk/asterisk.git] / addons / chan_mobile.c
1 /*
2  * Asterisk -- An open source telephony toolkit.
3  *
4  * Copyright (C) 1999 - 2006, Digium, Inc.
5  *
6  * Mark Spencer <markster@digium.com>
7  *
8  * See http://www.asterisk.org for more information about
9  * the Asterisk project. Please do not directly contact
10  * any of the maintainers of this project for assistance;
11  * the project provides a web site, mailing lists and IRC
12  * channels for your use.
13  *
14  * This program is free software, distributed under the terms of
15  * the GNU General Public License Version 2. See the LICENSE file
16  * at the top of the source tree.
17  */
18
19 /*! 
20  * \file
21  * \brief Bluetooth Mobile Device channel driver
22  *
23  * \author Dave Bowerman <david.bowerman@gmail.com>
24  *
25  * \ingroup channel_drivers
26  */
27
28 /*** MODULEINFO
29         <depend>bluetooth</depend>
30         <defaultenabled>no</defaultenabled>
31  ***/
32
33 #include "asterisk.h"
34
35 ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
36
37 #include <pthread.h>
38 #include <signal.h>
39
40 #include <bluetooth/bluetooth.h>
41 #include <bluetooth/hci.h>
42 #include <bluetooth/hci_lib.h>
43 #include <bluetooth/sdp.h>
44 #include <bluetooth/sdp_lib.h>
45 #include <bluetooth/rfcomm.h>
46 #include <bluetooth/sco.h>
47 #include <bluetooth/l2cap.h>
48
49 #include "asterisk/compat.h"
50 #include "asterisk/lock.h"
51 #include "asterisk/channel.h"
52 #include "asterisk/config.h"
53 #include "asterisk/logger.h"
54 #include "asterisk/module.h"
55 #include "asterisk/pbx.h"
56 #include "asterisk/options.h"
57 #include "asterisk/utils.h"
58 #include "asterisk/linkedlists.h"
59 #include "asterisk/cli.h"
60 #include "asterisk/devicestate.h"
61 #include "asterisk/causes.h"
62 #include "asterisk/dsp.h"
63 #include "asterisk/app.h"
64 #include "asterisk/manager.h"
65 #include "asterisk/io.h"
66
67 #define MBL_CONFIG "chan_mobile.conf"
68 #define MBL_CONFIG_OLD "mobile.conf"
69
70 #define DEVICE_FRAME_SIZE 48
71 #define DEVICE_FRAME_FORMAT AST_FORMAT_SLINEAR
72 #define CHANNEL_FRAME_SIZE 320
73
74 static format_t prefformat = DEVICE_FRAME_FORMAT;
75
76 static int discovery_interval = 60;                     /* The device discovery interval, default 60 seconds. */
77 static pthread_t discovery_thread = AST_PTHREADT_NULL;  /* The discovery thread */
78 static sdp_session_t *sdp_session;
79
80 AST_MUTEX_DEFINE_STATIC(unload_mutex);
81 static int unloading_flag = 0;
82 static inline int check_unloading(void);
83 static inline void set_unloading(void);
84
85 enum mbl_type {
86         MBL_TYPE_PHONE,
87         MBL_TYPE_HEADSET
88 };
89
90 struct adapter_pvt {
91         int dev_id;                                     /* device id */
92         int hci_socket;                                 /* device descriptor */
93         char id[31];                                    /* the 'name' from mobile.conf */
94         bdaddr_t addr;                                  /* adddress of adapter */
95         unsigned int inuse:1;                           /* are we in use ? */
96         unsigned int alignment_detection:1;             /* do alignment detection on this adpater? */
97         struct io_context *io;                          /*!< io context for audio connections */
98         struct io_context *accept_io;                   /*!< io context for sco listener */
99         int *sco_id;                                    /*!< the io context id of the sco listener socket */
100         int sco_socket;                                 /*!< sco listener socket */
101         pthread_t sco_listener_thread;                  /*!< sco listener thread */
102         AST_LIST_ENTRY(adapter_pvt) entry;
103 };
104
105 static AST_RWLIST_HEAD_STATIC(adapters, adapter_pvt);
106
107 struct msg_queue_entry;
108 struct hfp_pvt;
109 struct mbl_pvt {
110         struct ast_channel *owner;                      /* Channel we belong to, possibly NULL */
111         struct ast_frame fr;                            /* "null" frame */
112         ast_mutex_t lock;                               /*!< pvt lock */
113         /*! queue for messages we are expecting */
114         AST_LIST_HEAD_NOLOCK(msg_queue, msg_queue_entry) msg_queue;
115         enum mbl_type type;                             /* Phone or Headset */
116         char id[31];                                    /* The id from mobile.conf */
117         int group;                                      /* group number for group dialling */
118         bdaddr_t addr;                                  /* address of device */
119         struct adapter_pvt *adapter;                    /* the adapter we use */
120         char context[AST_MAX_CONTEXT];                  /* the context for incoming calls */
121         struct hfp_pvt *hfp;                            /*!< hfp pvt */
122         int rfcomm_port;                                /* rfcomm port number */
123         int rfcomm_socket;                              /* rfcomm socket descriptor */
124         char rfcomm_buf[256];
125         char io_buf[CHANNEL_FRAME_SIZE + AST_FRIENDLY_OFFSET];
126         struct ast_smoother *smoother;                  /* our smoother, for making 48 byte frames */
127         int sco_socket;                                 /* sco socket descriptor */
128         pthread_t monitor_thread;                       /* monitor thread handle */
129         int timeout;                                    /*!< used to set the timeout for rfcomm data (may be used in the future) */
130         unsigned int no_callsetup:1;
131         unsigned int has_sms:1;
132         unsigned int do_alignment_detection:1;
133         unsigned int alignment_detection_triggered:1;
134         unsigned int blackberry:1;
135         short alignment_samples[4];
136         int alignment_count;
137         int ring_sched_id;
138         struct ast_dsp *dsp;
139         struct sched_context *sched;
140
141         /* flags */
142         unsigned int outgoing:1;        /*!< outgoing call */
143         unsigned int incoming:1;        /*!< incoming call */
144         unsigned int outgoing_sms:1;    /*!< outgoing sms */
145         unsigned int incoming_sms:1;    /*!< outgoing sms */
146         unsigned int needcallerid:1;    /*!< we need callerid */
147         unsigned int needchup:1;        /*!< we need to send a chup */
148         unsigned int needring:1;        /*!< we need to send a RING */
149         unsigned int answered:1;        /*!< we sent/received an answer */
150         unsigned int connected:1;       /*!< do we have an rfcomm connection to a device */
151
152         AST_LIST_ENTRY(mbl_pvt) entry;
153 };
154
155 static AST_RWLIST_HEAD_STATIC(devices, mbl_pvt);
156
157 static int handle_response_ok(struct mbl_pvt *pvt, char *buf);
158 static int handle_response_error(struct mbl_pvt *pvt, char *buf);
159 static int handle_response_ciev(struct mbl_pvt *pvt, char *buf);
160 static int handle_response_clip(struct mbl_pvt *pvt, char *buf);
161 static int handle_response_ring(struct mbl_pvt *pvt, char *buf);
162 static int handle_response_cmti(struct mbl_pvt *pvt, char *buf);
163 static int handle_response_cmgr(struct mbl_pvt *pvt, char *buf);
164 static int handle_response_cusd(struct mbl_pvt *pvt, char *buf);
165 static int handle_sms_prompt(struct mbl_pvt *pvt, char *buf);
166
167 /* CLI stuff */
168 static char *handle_cli_mobile_show_devices(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a);
169 static char *handle_cli_mobile_search(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a);
170 static char *handle_cli_mobile_rfcomm(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a);
171 static char *handle_cli_mobile_cusd(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a);
172
173 static struct ast_cli_entry mbl_cli[] = {
174         AST_CLI_DEFINE(handle_cli_mobile_show_devices, "Show Bluetooth Cell / Mobile devices"),
175         AST_CLI_DEFINE(handle_cli_mobile_search,       "Search for Bluetooth Cell / Mobile devices"),
176         AST_CLI_DEFINE(handle_cli_mobile_rfcomm,       "Send commands to the rfcomm port for debugging"),
177         AST_CLI_DEFINE(handle_cli_mobile_cusd,         "Send CUSD commands to the mobile"),
178 };
179
180 /* App stuff */
181 static char *app_mblstatus = "MobileStatus";
182 static char *mblstatus_synopsis = "MobileStatus(Device,Variable)";
183 static char *mblstatus_desc =
184 "MobileStatus(Device,Variable)\n"
185 "  Device - Id of mobile device from mobile.conf\n"
186 "  Variable - Variable to store status in will be 1-3.\n"
187 "             In order, Disconnected, Connected & Free, Connected & Busy.\n";
188
189 static char *app_mblsendsms = "MobileSendSMS";
190 static char *mblsendsms_synopsis = "MobileSendSMS(Device,Dest,Message)";
191 static char *mblsendsms_desc =
192 "MobileSendSms(Device,Dest,Message)\n"
193 "  Device - Id of device from mobile.conf\n"
194 "  Dest - destination\n"
195 "  Message - text of the message\n";
196
197 static struct ast_channel *mbl_new(int state, struct mbl_pvt *pvt, char *cid_num,
198                 const struct ast_channel *requestor);
199 static struct ast_channel *mbl_request(const char *type, format_t format,
200                 const struct ast_channel *requestor, void *data, int *cause);
201 static int mbl_call(struct ast_channel *ast, char *dest, int timeout);
202 static int mbl_hangup(struct ast_channel *ast);
203 static int mbl_answer(struct ast_channel *ast);
204 static int mbl_digit_end(struct ast_channel *ast, char digit, unsigned int duration);
205 static struct ast_frame *mbl_read(struct ast_channel *ast);
206 static int mbl_write(struct ast_channel *ast, struct ast_frame *frame);
207 static int mbl_fixup(struct ast_channel *oldchan, struct ast_channel *newchan);
208 static int mbl_devicestate(void *data);
209
210 static void do_alignment_detection(struct mbl_pvt *pvt, char *buf, int buflen);
211
212 static int mbl_queue_control(struct mbl_pvt *pvt, enum ast_control_frame_type control);
213 static int mbl_queue_hangup(struct mbl_pvt *pvt);
214 static int mbl_ast_hangup(struct mbl_pvt *pvt);
215 static int mbl_has_service(struct mbl_pvt *pvt);
216
217 static int rfcomm_connect(bdaddr_t src, bdaddr_t dst, int remote_channel);
218 static int rfcomm_write(int rsock, char *buf);
219 static int rfcomm_write_full(int rsock, char *buf, size_t count);
220 static int rfcomm_wait(int rsock, int *ms);
221 static ssize_t rfcomm_read(int rsock, char *buf, size_t count);
222
223 static int sco_connect(bdaddr_t src, bdaddr_t dst);
224 static int sco_write(int s, char *buf, int len);
225 static int sco_accept(int *id, int fd, short events, void *data);
226 static int sco_bind(struct adapter_pvt *adapter);
227
228 static void *do_sco_listen(void *data);
229 static int sdp_search(char *addr, int profile);
230
231 static int headset_send_ring(const void *data);
232
233 /*
234  * bluetooth handsfree profile helpers
235  */
236
237 #define HFP_HF_ECNR     (1 << 0)
238 #define HFP_HF_CW       (1 << 1)
239 #define HFP_HF_CID      (1 << 2)
240 #define HFP_HF_VOICE    (1 << 3)
241 #define HFP_HF_VOLUME   (1 << 4)
242 #define HFP_HF_STATUS   (1 << 5)
243 #define HFP_HF_CONTROL  (1 << 6)
244
245 #define HFP_AG_CW       (1 << 0)
246 #define HFP_AG_ECNR     (1 << 1)
247 #define HFP_AG_VOICE    (1 << 2)
248 #define HFP_AG_RING     (1 << 3)
249 #define HFP_AG_TAG      (1 << 4)
250 #define HFP_AG_REJECT   (1 << 5)
251 #define HFP_AG_STATUS   (1 << 6)
252 #define HFP_AG_CONTROL  (1 << 7)
253 #define HFP_AG_ERRORS   (1 << 8)
254
255 #define HFP_CIND_UNKNOWN        -1
256 #define HFP_CIND_NONE           0
257 #define HFP_CIND_SERVICE        1
258 #define HFP_CIND_CALL           2
259 #define HFP_CIND_CALLSETUP      3
260 #define HFP_CIND_CALLHELD       4
261 #define HFP_CIND_SIGNAL         5
262 #define HFP_CIND_ROAM           6
263 #define HFP_CIND_BATTCHG        7
264
265 /* call indicator values */
266 #define HFP_CIND_CALL_NONE      0
267 #define HFP_CIND_CALL_ACTIVE    1
268
269 /* callsetup indicator values */
270 #define HFP_CIND_CALLSETUP_NONE         0
271 #define HFP_CIND_CALLSETUP_INCOMING     1
272 #define HFP_CIND_CALLSETUP_OUTGOING     2
273 #define HFP_CIND_CALLSETUP_ALERTING     3
274
275 /* service indicator values */
276 #define HFP_CIND_SERVICE_NONE           0
277 #define HFP_CIND_SERVICE_AVAILABLE      1
278
279 /*!
280  * \brief This struct holds HFP features that we support.
281  */
282 struct hfp_hf {
283         int ecnr:1;     /*!< echo-cancel/noise reduction */
284         int cw:1;       /*!< call waiting and three way calling */
285         int cid:1;      /*!< cli presentation (callier id) */
286         int voice:1;    /*!< voice recognition activation */
287         int volume:1;   /*!< remote volume control */
288         int status:1;   /*!< enhanced call status */
289         int control:1;  /*!< enhanced call control*/
290 };
291
292 /*!
293  * \brief This struct holds HFP features the AG supports.
294  */
295 struct hfp_ag {
296         int cw:1;       /*!< three way calling */
297         int ecnr:1;     /*!< echo-cancel/noise reduction */
298         int voice:1;    /*!< voice recognition */
299         int ring:1;     /*!< in band ring tone capability */
300         int tag:1;      /*!< attach a number to a voice tag */
301         int reject:1;   /*!< ability to reject a call */
302         int status:1;   /*!< enhanced call status */
303         int control:1;  /*!< enhanced call control*/
304         int errors:1;   /*!< extended error result codes*/
305 };
306
307 /*!
308  * \brief This struct holds mappings for indications.
309  */
310 struct hfp_cind {
311         int service;    /*!< whether we have service or not */
312         int call;       /*!< call state */
313         int callsetup;  /*!< bluetooth call setup indications */
314         int callheld;   /*!< bluetooth call hold indications */
315         int signal;     /*!< signal strength */
316         int roam;       /*!< roaming indicator */
317         int battchg;    /*!< battery charge indicator */
318 };
319
320
321 /*!
322  * \brief This struct holds state information about the current hfp connection.
323  */
324 struct hfp_pvt {
325         struct mbl_pvt *owner;          /*!< the mbl_pvt struct that owns this struct */
326         int initialized:1;              /*!< whether a service level connection exists or not */
327         int nocallsetup:1;              /*!< whether we detected a callsetup indicator */
328         struct hfp_ag brsf;             /*!< the supported feature set of the AG */
329         int cind_index[16];             /*!< the cind/ciev index to name mapping for this AG */
330         int cind_state[16];             /*!< the cind/ciev state for this AG */
331         struct hfp_cind cind_map;       /*!< the cind name to index mapping for this AG */
332         int rsock;                      /*!< our rfcomm socket */
333         int rport;                      /*!< our rfcomm port */
334 };
335
336
337 /* Our supported features.
338  * we only support caller id
339  */
340 static struct hfp_hf hfp_our_brsf = {
341         .ecnr = 0,
342         .cw = 0,
343         .cid = 1,
344         .voice = 0,
345         .volume = 0,
346         .status = 0,
347         .control = 0,
348 };
349
350
351 static int hfp_parse_ciev(struct hfp_pvt *hfp, char *buf, int *value);
352 static char *hfp_parse_clip(struct hfp_pvt *hfp, char *buf);
353 static int hfp_parse_cmti(struct hfp_pvt *hfp, char *buf);
354 static int hfp_parse_cmgr(struct hfp_pvt *hfp, char *buf, char **from_number, char **text);
355 static int hfp_parse_brsf(struct hfp_pvt *hfp, const char *buf);
356 static int hfp_parse_cind(struct hfp_pvt *hfp, char *buf);
357 static int hfp_parse_cind_test(struct hfp_pvt *hfp, char *buf);
358 static char *hfp_parse_cusd(struct hfp_pvt *hfp, char *buf);
359
360 static int hfp_brsf2int(struct hfp_hf *hf);
361 static struct hfp_ag *hfp_int2brsf(int brsf, struct hfp_ag *ag);
362
363 static int hfp_send_brsf(struct hfp_pvt *hfp, struct hfp_hf *brsf);
364 static int hfp_send_cind(struct hfp_pvt *hfp);
365 static int hfp_send_cind_test(struct hfp_pvt *hfp);
366 static int hfp_send_cmer(struct hfp_pvt *hfp, int status);
367 static int hfp_send_clip(struct hfp_pvt *hfp, int status);
368 static int hfp_send_vgs(struct hfp_pvt *hfp, int value);
369
370 #if 0
371 static int hfp_send_vgm(struct hfp_pvt *hfp, int value);
372 #endif
373 static int hfp_send_dtmf(struct hfp_pvt *hfp, char digit);
374 static int hfp_send_cmgf(struct hfp_pvt *hfp, int mode);
375 static int hfp_send_cnmi(struct hfp_pvt *hfp);
376 static int hfp_send_cmgr(struct hfp_pvt *hfp, int index);
377 static int hfp_send_cmgs(struct hfp_pvt *hfp, const char *number);
378 static int hfp_send_sms_text(struct hfp_pvt *hfp, const char *message);
379 static int hfp_send_chup(struct hfp_pvt *hfp);
380 static int hfp_send_atd(struct hfp_pvt *hfp, const char *number);
381 static int hfp_send_ata(struct hfp_pvt *hfp);
382 static int hfp_send_cusd(struct hfp_pvt *hfp, const char *code);
383
384 /*
385  * bluetooth headset profile helpers
386  */
387 static int hsp_send_ok(int rsock);
388 static int hsp_send_error(int rsock);
389 static int hsp_send_vgs(int rsock, int gain);
390 static int hsp_send_vgm(int rsock, int gain);
391 static int hsp_send_ring(int rsock);
392
393
394 /*
395  * Hayes AT command helpers
396  */
397 typedef enum {
398         /* errors */
399         AT_PARSE_ERROR = -2,
400         AT_READ_ERROR = -1,
401         AT_UNKNOWN = 0,
402         /* at responses */
403         AT_OK,
404         AT_ERROR,
405         AT_RING,
406         AT_BRSF,
407         AT_CIND,
408         AT_CIEV,
409         AT_CLIP,
410         AT_CMTI,
411         AT_CMGR,
412         AT_SMS_PROMPT,
413         AT_CMS_ERROR,
414         /* at commands */
415         AT_A,
416         AT_D,
417         AT_CHUP,
418         AT_CKPD,
419         AT_CMGS,
420         AT_VGM,
421         AT_VGS,
422         AT_VTS,
423         AT_CMGF,
424         AT_CNMI,
425         AT_CMER,
426         AT_CIND_TEST,
427         AT_CUSD,
428 } at_message_t;
429
430 static int at_match_prefix(char *buf, char *prefix);
431 static at_message_t at_read_full(int rsock, char *buf, size_t count);
432 static inline const char *at_msg2str(at_message_t msg);
433
434 struct msg_queue_entry {
435         at_message_t expected;
436         at_message_t response_to;
437         void *data;
438
439         AST_LIST_ENTRY(msg_queue_entry) entry;
440 };
441
442 static int msg_queue_push(struct mbl_pvt *pvt, at_message_t expect, at_message_t response_to);
443 static int msg_queue_push_data(struct mbl_pvt *pvt, at_message_t expect, at_message_t response_to, void *data);
444 static struct msg_queue_entry *msg_queue_pop(struct mbl_pvt *pvt);
445 static void msg_queue_free_and_pop(struct mbl_pvt *pvt);
446 static void msg_queue_flush(struct mbl_pvt *pvt);
447 static struct msg_queue_entry *msg_queue_head(struct mbl_pvt *pvt);
448
449 /*
450  * channel stuff
451  */
452
453 static const struct ast_channel_tech mbl_tech = {
454         .type = "Mobile",
455         .description = "Bluetooth Mobile Device Channel Driver",
456         .capabilities = AST_FORMAT_SLINEAR,
457         .requester = mbl_request,
458         .call = mbl_call,
459         .hangup = mbl_hangup,
460         .answer = mbl_answer,
461         .send_digit_end = mbl_digit_end,
462         .read = mbl_read,
463         .write = mbl_write,
464         .fixup = mbl_fixup,
465         .devicestate = mbl_devicestate
466 };
467
468 /* CLI Commands implementation */
469
470 static char *handle_cli_mobile_show_devices(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
471 {
472         struct mbl_pvt *pvt;
473         char bdaddr[18];
474         char group[6];
475
476 #define FORMAT1 "%-15.15s %-17.17s %-5.5s %-15.15s %-9.9s %-10.10s %-3.3s\n"
477
478         switch (cmd) {
479         case CLI_INIT:
480                 e->command = "mobile show devices";
481                 e->usage =
482                         "Usage: mobile show devices\n"
483                         "       Shows the state of Bluetooth Cell / Mobile devices.\n";
484                 return NULL;
485         case CLI_GENERATE:
486                 return NULL;
487         }
488
489         if (a->argc != 3)
490                 return CLI_SHOWUSAGE;
491
492         ast_cli(a->fd, FORMAT1, "ID", "Address", "Group", "Adapter", "Connected", "State", "SMS");
493         AST_RWLIST_RDLOCK(&devices);
494         AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
495                 ast_mutex_lock(&pvt->lock);
496                 ba2str(&pvt->addr, bdaddr);
497                 snprintf(group, sizeof(group), "%d", pvt->group);
498                 ast_cli(a->fd, FORMAT1,
499                                 pvt->id,
500                                 bdaddr,
501                                 group,
502                                 pvt->adapter->id,
503                                 pvt->connected ? "Yes" : "No",
504                                 (!pvt->connected) ? "None" : (pvt->owner) ? "Busy" : (pvt->outgoing_sms || pvt->incoming_sms) ? "SMS" : (mbl_has_service(pvt)) ? "Free" : "No Service",
505                                 (pvt->has_sms) ? "Yes" : "No"
506                        );
507                 ast_mutex_unlock(&pvt->lock);
508         }
509         AST_RWLIST_UNLOCK(&devices);
510
511 #undef FORMAT1
512
513         return CLI_SUCCESS;
514 }
515
516 static char *handle_cli_mobile_search(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
517 {
518         struct adapter_pvt *adapter;
519         inquiry_info *ii = NULL;
520         int max_rsp, num_rsp;
521         int len, flags;
522         int i, phport, hsport;
523         char addr[19] = {0};
524         char name[31] = {0};
525
526 #define FORMAT1 "%-17.17s %-30.30s %-6.6s %-7.7s %-4.4s\n"
527 #define FORMAT2 "%-17.17s %-30.30s %-6.6s %-7.7s %d\n"
528
529         switch (cmd) {
530         case CLI_INIT:
531                 e->command = "mobile search";
532                 e->usage =
533                         "Usage: mobile search\n"
534                         "       Searches for Bluetooth Cell / Mobile devices in range.\n";
535                 return NULL;
536         case CLI_GENERATE:
537                 return NULL;
538         }
539
540         if (a->argc != 2)
541                 return CLI_SHOWUSAGE;
542
543         /* find a free adapter */
544         AST_RWLIST_RDLOCK(&adapters);
545         AST_RWLIST_TRAVERSE(&adapters, adapter, entry) {
546                 if (!adapter->inuse)
547                         break;
548         }
549         AST_RWLIST_UNLOCK(&adapters);
550
551         if (!adapter) {
552                 ast_cli(a->fd, "All Bluetooth adapters are in use at this time.\n");
553                 return CLI_SUCCESS;
554         }
555
556         len  = 8;
557         max_rsp = 255;
558         flags = IREQ_CACHE_FLUSH;
559
560         ii = alloca(max_rsp * sizeof(inquiry_info));
561         num_rsp = hci_inquiry(adapter->dev_id, len, max_rsp, NULL, &ii, flags);
562         if (num_rsp > 0) {
563                 ast_cli(a->fd, FORMAT1, "Address", "Name", "Usable", "Type", "Port");
564                 for (i = 0; i < num_rsp; i++) {
565                         ba2str(&(ii + i)->bdaddr, addr);
566                         name[0] = 0x00;
567                         if (hci_read_remote_name(adapter->hci_socket, &(ii + i)->bdaddr, sizeof(name) - 1, name, 0) < 0)
568                                 strcpy(name, "[unknown]");
569                         phport = sdp_search(addr, HANDSFREE_AGW_PROFILE_ID);
570                         if (!phport)
571                                 hsport = sdp_search(addr, HEADSET_PROFILE_ID);
572                         else
573                                 hsport = 0;
574                         ast_cli(a->fd, FORMAT2, addr, name, (phport > 0 || hsport > 0) ? "Yes" : "No",
575                                 (phport > 0) ? "Phone" : "Headset", (phport > 0) ? phport : hsport);
576                 }
577         } else
578                 ast_cli(a->fd, "No Bluetooth Cell / Mobile devices found.\n");
579
580 #undef FORMAT1
581 #undef FORMAT2
582
583         return CLI_SUCCESS;
584 }
585
586 static char *handle_cli_mobile_rfcomm(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
587 {
588         char buf[128];
589         struct mbl_pvt *pvt = NULL;
590
591         switch (cmd) {
592         case CLI_INIT:
593                 e->command = "mobile rfcomm";
594                 e->usage =
595                         "Usage: mobile rfcomm <device ID> <command>\n"
596                         "       Send <command> to the rfcomm port on the device\n"
597                         "       with the specified <device ID>.\n";
598                 return NULL;
599         case CLI_GENERATE:
600                 return NULL;
601         }
602
603         if (a->argc != 4)
604                 return CLI_SHOWUSAGE;
605
606         AST_RWLIST_RDLOCK(&devices);
607         AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
608                 if (!strcmp(pvt->id, a->argv[2]))
609                         break;
610         }
611         AST_RWLIST_UNLOCK(&devices);
612
613         if (!pvt) {
614                 ast_cli(a->fd, "Device %s not found.\n", a->argv[2]);
615                 goto e_return;
616         }
617
618         ast_mutex_lock(&pvt->lock);
619         if (!pvt->connected) {
620                 ast_cli(a->fd, "Device %s not connected.\n", a->argv[2]);
621                 goto e_unlock_pvt;
622         }
623
624         snprintf(buf, sizeof(buf), "%s\r", a->argv[3]);
625         rfcomm_write(pvt->rfcomm_socket, buf);
626         msg_queue_push(pvt, AT_OK, AT_UNKNOWN);
627
628 e_unlock_pvt:
629         ast_mutex_unlock(&pvt->lock);
630 e_return:
631         return CLI_SUCCESS;
632 }
633
634 static char *handle_cli_mobile_cusd(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
635 {
636         char buf[128];
637         struct mbl_pvt *pvt = NULL;
638
639         switch (cmd) {
640         case CLI_INIT:
641                 e->command = "mobile cusd";
642                 e->usage =
643                         "Usage: mobile cusd <device ID> <command>\n"
644                         "       Send cusd <command> to the rfcomm port on the device\n"
645                         "       with the specified <device ID>.\n";
646                 return NULL;
647         case CLI_GENERATE:
648                 return NULL;
649         }
650
651         if (a->argc != 4)
652                 return CLI_SHOWUSAGE;
653
654         AST_RWLIST_RDLOCK(&devices);
655         AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
656                 if (!strcmp(pvt->id, a->argv[2]))
657                         break;
658         }
659         AST_RWLIST_UNLOCK(&devices);
660
661         if (!pvt) {
662                 ast_cli(a->fd, "Device %s not found.\n", a->argv[2]);
663                 goto e_return;
664         }
665
666         ast_mutex_lock(&pvt->lock);
667         if (!pvt->connected) {
668                 ast_cli(a->fd, "Device %s not connected.\n", a->argv[2]);
669                 goto e_unlock_pvt;
670         }
671
672         snprintf(buf, sizeof(buf), "%s", a->argv[3]);
673         if (hfp_send_cusd(pvt->hfp, buf) || msg_queue_push(pvt, AT_OK, AT_CUSD)) {
674                 ast_cli(a->fd, "[%s] error sending CUSD\n", pvt->id);
675                 goto e_unlock_pvt;
676         }
677
678 e_unlock_pvt:
679         ast_mutex_unlock(&pvt->lock);
680 e_return:
681         return CLI_SUCCESS;
682 }
683
684 /*
685
686         Dialplan applications implementation
687
688 */
689
690 static int mbl_status_exec(struct ast_channel *ast, const char *data)
691 {
692
693         struct mbl_pvt *pvt;
694         char *parse;
695         int stat;
696         char status[2];
697
698         AST_DECLARE_APP_ARGS(args,
699                 AST_APP_ARG(device);
700                 AST_APP_ARG(variable);
701         );
702
703         if (ast_strlen_zero(data))
704                 return -1;
705
706         parse = ast_strdupa(data);
707
708         AST_STANDARD_APP_ARGS(args, parse);
709
710         if (ast_strlen_zero(args.device) || ast_strlen_zero(args.variable))
711                 return -1;
712
713         stat = 1;
714
715         AST_RWLIST_RDLOCK(&devices);
716         AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
717                 if (!strcmp(pvt->id, args.device))
718                         break;
719         }
720         AST_RWLIST_UNLOCK(&devices);
721
722         if (pvt) {
723                 ast_mutex_lock(&pvt->lock);
724                 if (pvt->connected)
725                         stat = 2;
726                 if (pvt->owner)
727                         stat = 3;
728                 ast_mutex_unlock(&pvt->lock);
729         }
730
731         snprintf(status, sizeof(status), "%d", stat);
732         pbx_builtin_setvar_helper(ast, args.variable, status);
733
734         return 0;
735
736 }
737
738 static int mbl_sendsms_exec(struct ast_channel *ast, const char *data)
739 {
740
741         struct mbl_pvt *pvt;
742         char *parse, *message;
743
744         AST_DECLARE_APP_ARGS(args,
745                 AST_APP_ARG(device);
746                 AST_APP_ARG(dest);
747                 AST_APP_ARG(message);
748         );
749
750         if (ast_strlen_zero(data))
751                 return -1;
752
753         parse = ast_strdupa(data);
754
755         AST_STANDARD_APP_ARGS(args, parse);
756
757         if (ast_strlen_zero(args.device)) {
758                 ast_log(LOG_ERROR,"NULL device for message -- SMS will not be sent.\n");
759                 return -1;
760         }
761
762         if (ast_strlen_zero(args.dest)) {
763                 ast_log(LOG_ERROR,"NULL destination for message -- SMS will not be sent.\n");
764                 return -1;
765         }
766
767         if (ast_strlen_zero(args.message)) {
768                 ast_log(LOG_ERROR,"NULL Message to be sent -- SMS will not be sent.\n");
769                 return -1;
770         }
771
772         AST_RWLIST_RDLOCK(&devices);
773         AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
774                 if (!strcmp(pvt->id, args.device))
775                         break;
776         }
777         AST_RWLIST_UNLOCK(&devices);
778
779         if (!pvt) {
780                 ast_log(LOG_ERROR,"Bluetooth device %s wasn't found in the list -- SMS will not be sent.\n", args.device);
781                 goto e_return;
782         }
783
784         ast_mutex_lock(&pvt->lock);
785         if (!pvt->connected) {
786                 ast_log(LOG_ERROR,"Bluetooth device %s wasn't connected -- SMS will not be sent.\n", args.device);
787                 goto e_unlock_pvt;
788         }
789
790         if (!pvt->has_sms) {
791                 ast_log(LOG_ERROR,"Bluetooth device %s doesn't handle SMS -- SMS will not be sent.\n", args.device);
792                 goto e_unlock_pvt;
793         }
794
795         message = ast_strdup(args.message);
796
797         if (hfp_send_cmgs(pvt->hfp, args.dest)
798                 || msg_queue_push_data(pvt, AT_SMS_PROMPT, AT_CMGS, message)) {
799
800                 ast_log(LOG_ERROR, "[%s] problem sending SMS message\n", pvt->id);
801                 goto e_free_message;
802         }
803
804         ast_mutex_unlock(&pvt->lock);
805
806         return 0;
807
808 e_free_message:
809         ast_free(message);
810 e_unlock_pvt:
811         ast_mutex_unlock(&pvt->lock);
812 e_return:
813         return -1;
814 }
815
816 /*
817
818         Channel Driver callbacks
819
820 */
821
822 static struct ast_channel *mbl_new(int state, struct mbl_pvt *pvt, char *cid_num,
823                 const struct ast_channel *requestor)
824 {
825
826         struct ast_channel *chn;
827
828         pvt->answered = 0;
829         pvt->alignment_count = 0;
830         pvt->alignment_detection_triggered = 0;
831         if (pvt->adapter->alignment_detection)
832                 pvt->do_alignment_detection = 1;
833         else
834                 pvt->do_alignment_detection = 0;
835
836         ast_smoother_reset(pvt->smoother, DEVICE_FRAME_SIZE);
837         ast_dsp_digitreset(pvt->dsp);
838
839         chn = ast_channel_alloc(1, state, cid_num, pvt->id, 0, 0, pvt->context,
840                         requestor ? requestor->linkedid : "", 0,
841                         "Mobile/%s-%04lx", pvt->id, ast_random() & 0xffff);
842         if (!chn) {
843                 goto e_return;
844         }
845
846         chn->tech = &mbl_tech;
847         chn->nativeformats = prefformat;
848         chn->rawreadformat = prefformat;
849         chn->rawwriteformat = prefformat;
850         chn->writeformat = prefformat;
851         chn->readformat = prefformat;
852         chn->tech_pvt = pvt;
853
854         if (state == AST_STATE_RING)
855                 chn->rings = 1;
856
857         ast_string_field_set(chn, language, "en");
858         pvt->owner = chn;
859
860         if (pvt->sco_socket != -1) {
861                 ast_channel_set_fd(chn, 0, pvt->sco_socket);
862         }
863
864         return chn;
865
866 e_return:
867         return NULL;
868 }
869
870 static struct ast_channel *mbl_request(const char *type, format_t format,
871                 const struct ast_channel *requestor, void *data, int *cause)
872 {
873
874         struct ast_channel *chn = NULL;
875         struct mbl_pvt *pvt;
876         char *dest_dev = NULL;
877         char *dest_num = NULL;
878         format_t oldformat;
879         int group = -1;
880
881         if (!data) {
882                 ast_log(LOG_WARNING, "Channel requested with no data\n");
883                 *cause = AST_CAUSE_INCOMPATIBLE_DESTINATION;
884                 return NULL;
885         }
886
887         oldformat = format;
888         format &= (AST_FORMAT_SLINEAR);
889         if (!format) {
890                 ast_log(LOG_WARNING, "Asked to get a channel of unsupported format '%s'\n", ast_getformatname(oldformat));
891                 *cause = AST_CAUSE_FACILITY_NOT_IMPLEMENTED;
892                 return NULL;
893         }
894
895         dest_dev = ast_strdupa((char *)data);
896
897         dest_num = strchr(dest_dev, '/');
898         if (dest_num)
899                 *dest_num++ = 0x00;
900
901         if (((dest_dev[0] == 'g') || (dest_dev[0] == 'G')) && ((dest_dev[1] >= '0') && (dest_dev[1] <= '9'))) {
902                 group = atoi(&dest_dev[1]);
903         }
904
905         /* Find requested device and make sure it's connected. */
906         AST_RWLIST_RDLOCK(&devices);
907         AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
908                 if (group > -1 && pvt->group == group && pvt->connected && !pvt->owner) {
909                         if (!mbl_has_service(pvt)) {
910                                 continue;
911                         }
912
913                         break;
914                 } else if (!strcmp(pvt->id, dest_dev)) {
915                         break;
916                 }
917         }
918         AST_RWLIST_UNLOCK(&devices);
919         if (!pvt || !pvt->connected || pvt->owner) {
920                 ast_log(LOG_WARNING, "Request to call on device %s which is not connected / already in use.\n", dest_dev);
921                 *cause = AST_CAUSE_REQUESTED_CHAN_UNAVAIL;
922                 return NULL;
923         }
924
925         if ((pvt->type == MBL_TYPE_PHONE) && !dest_num) {
926                 ast_log(LOG_WARNING, "Can't determine destination number.\n");
927                 *cause = AST_CAUSE_INCOMPATIBLE_DESTINATION;
928                 return NULL;
929         }
930
931         ast_mutex_lock(&pvt->lock);
932         chn = mbl_new(AST_STATE_DOWN, pvt, NULL, requestor);
933         ast_mutex_unlock(&pvt->lock);
934         if (!chn) {
935                 ast_log(LOG_WARNING, "Unable to allocate channel structure.\n");
936                 *cause = AST_CAUSE_REQUESTED_CHAN_UNAVAIL;
937                 return NULL;
938         }
939
940         return chn;
941
942 }
943
944 static int mbl_call(struct ast_channel *ast, char *dest, int timeout)
945 {
946
947         struct mbl_pvt *pvt;
948         char *dest_dev = NULL;
949         char *dest_num = NULL;
950
951         dest_dev = ast_strdupa((char *)dest);
952
953         pvt = ast->tech_pvt;
954
955         if (pvt->type == MBL_TYPE_PHONE) {
956                 dest_num = strchr(dest_dev, '/');
957                 if (!dest_num) {
958                         ast_log(LOG_WARNING, "Cant determine destination number.\n");
959                         return -1;
960                 }
961                 *dest_num++ = 0x00;
962         }
963
964         if ((ast->_state != AST_STATE_DOWN) && (ast->_state != AST_STATE_RESERVED)) {
965                 ast_log(LOG_WARNING, "mbl_call called on %s, neither down nor reserved\n", ast->name);
966                 return -1;
967         }
968
969         ast_debug(1, "Calling %s on %s\n", dest, ast->name);
970
971         ast_mutex_lock(&pvt->lock);
972         if (pvt->type == MBL_TYPE_PHONE) {
973                 if (hfp_send_atd(pvt->hfp, dest_num)) {
974                         ast_mutex_unlock(&pvt->lock);
975                         ast_log(LOG_ERROR, "error sending ATD command on %s\n", pvt->id);
976                         return -1;
977                 }
978                 pvt->needchup = 1;
979                 msg_queue_push(pvt, AT_OK, AT_D);
980         } else {
981                 if (hsp_send_ring(pvt->rfcomm_socket)) {
982                         ast_log(LOG_ERROR, "[%s] error ringing device\n", pvt->id);
983                         ast_mutex_unlock(&pvt->lock);
984                         return -1;
985                 }
986
987                 if ((pvt->ring_sched_id = ast_sched_add(pvt->sched, 6000, headset_send_ring, pvt)) == -1) {
988                         ast_log(LOG_ERROR, "[%s] error ringing device\n", pvt->id);
989                         ast_mutex_unlock(&pvt->lock);
990                         return -1;
991                 }
992
993                 pvt->outgoing = 1;
994                 pvt->needring = 1;
995         }
996         ast_mutex_unlock(&pvt->lock);
997
998         return 0;
999
1000 }
1001
1002 static int mbl_hangup(struct ast_channel *ast)
1003 {
1004
1005         struct mbl_pvt *pvt;
1006
1007         if (!ast->tech_pvt) {
1008                 ast_log(LOG_WARNING, "Asked to hangup channel not connected\n");
1009                 return 0;
1010         }
1011         pvt = ast->tech_pvt;
1012
1013         ast_debug(1, "[%s] hanging up device\n", pvt->id);
1014
1015         ast_mutex_lock(&pvt->lock);
1016         ast_channel_set_fd(ast, 0, -1);
1017         close(pvt->sco_socket);
1018         pvt->sco_socket = -1;
1019
1020         if (pvt->needchup) {
1021                 hfp_send_chup(pvt->hfp);
1022                 msg_queue_push(pvt, AT_OK, AT_CHUP);
1023                 pvt->needchup = 0;
1024         }
1025
1026         pvt->outgoing = 0;
1027         pvt->incoming = 0;
1028         pvt->needring = 0;
1029         pvt->owner = NULL;
1030         ast->tech_pvt = NULL;
1031
1032         ast_mutex_unlock(&pvt->lock);
1033
1034         ast_setstate(ast, AST_STATE_DOWN);
1035
1036         return 0;
1037
1038 }
1039
1040 static int mbl_answer(struct ast_channel *ast)
1041 {
1042
1043         struct mbl_pvt *pvt;
1044
1045         pvt = ast->tech_pvt;
1046
1047         if (pvt->type == MBL_TYPE_HEADSET)
1048                 return 0;
1049
1050         ast_mutex_lock(&pvt->lock);
1051         if (pvt->incoming) {
1052                 hfp_send_ata(pvt->hfp);
1053                 msg_queue_push(pvt, AT_OK, AT_A);
1054                 pvt->answered = 1;
1055         }
1056         ast_mutex_unlock(&pvt->lock);
1057
1058         return 0;
1059
1060 }
1061
1062 static int mbl_digit_end(struct ast_channel *ast, char digit, unsigned int duration)
1063 {
1064         struct mbl_pvt *pvt = ast->tech_pvt;
1065
1066         if (pvt->type == MBL_TYPE_HEADSET)
1067                 return 0;
1068
1069         ast_mutex_lock(&pvt->lock);
1070         if (hfp_send_dtmf(pvt->hfp, digit)) {
1071                 ast_mutex_unlock(&pvt->lock);
1072                 ast_debug(1, "[%s] error sending digit %c\n", pvt->id, digit);
1073                 return -1;
1074         }
1075         msg_queue_push(pvt, AT_OK, AT_VTS);
1076         ast_mutex_unlock(&pvt->lock);
1077
1078         ast_debug(1, "[%s] dialed %c\n", pvt->id, digit);
1079
1080         return 0;
1081 }
1082
1083 static struct ast_frame *mbl_read(struct ast_channel *ast)
1084 {
1085
1086         struct mbl_pvt *pvt = ast->tech_pvt;
1087         struct ast_frame *fr = &ast_null_frame;
1088         int r;
1089
1090         ast_debug(3, "*** mbl_read()\n");
1091
1092         while (ast_mutex_trylock(&pvt->lock)) {
1093                 CHANNEL_DEADLOCK_AVOIDANCE(ast);
1094         }
1095
1096         if (!pvt->owner || pvt->sco_socket == -1) {
1097                 goto e_return;
1098         }
1099
1100         memset(&pvt->fr, 0x00, sizeof(struct ast_frame));
1101         pvt->fr.frametype = AST_FRAME_VOICE;
1102         pvt->fr.subclass.codec = DEVICE_FRAME_FORMAT;
1103         pvt->fr.src = "Mobile";
1104         pvt->fr.offset = AST_FRIENDLY_OFFSET;
1105         pvt->fr.mallocd = 0;
1106         pvt->fr.delivery.tv_sec = 0;
1107         pvt->fr.delivery.tv_usec = 0;
1108         pvt->fr.data.ptr = pvt->io_buf + AST_FRIENDLY_OFFSET;
1109
1110         if ((r = read(pvt->sco_socket, pvt->fr.data.ptr, DEVICE_FRAME_SIZE)) == -1) {
1111                 if (errno != EAGAIN && errno != EINTR) {
1112                         ast_debug(1, "[%s] read error %d, going to wait for new connection\n", pvt->id, errno);
1113                         close(pvt->sco_socket);
1114                         pvt->sco_socket = -1;
1115                         ast_channel_set_fd(ast, 0, -1);
1116                 }
1117                 goto e_return;
1118         }
1119
1120         pvt->fr.datalen = r;
1121         pvt->fr.samples = r / 2;
1122
1123         if (pvt->do_alignment_detection)
1124                 do_alignment_detection(pvt, pvt->fr.data.ptr, r);
1125
1126         fr = ast_dsp_process(ast, pvt->dsp, &pvt->fr);
1127
1128         ast_mutex_unlock(&pvt->lock);
1129
1130         return fr;
1131
1132 e_return:
1133         ast_mutex_unlock(&pvt->lock);
1134         return fr;
1135 }
1136
1137 static int mbl_write(struct ast_channel *ast, struct ast_frame *frame)
1138 {
1139
1140         struct mbl_pvt *pvt = ast->tech_pvt;
1141         struct ast_frame *f;
1142
1143         ast_debug(3, "*** mbl_write\n");
1144
1145         if (frame->frametype != AST_FRAME_VOICE) {
1146                 return 0;
1147         }
1148
1149         while (ast_mutex_trylock(&pvt->lock)) {
1150                 CHANNEL_DEADLOCK_AVOIDANCE(ast);
1151         }
1152
1153         ast_smoother_feed(pvt->smoother, frame);
1154
1155         while ((f = ast_smoother_read(pvt->smoother))) {
1156                 sco_write(pvt->sco_socket, f->data.ptr, f->datalen);
1157                 ast_frfree(f);
1158         }
1159
1160         ast_mutex_unlock(&pvt->lock);
1161
1162         return 0;
1163
1164 }
1165
1166 static int mbl_fixup(struct ast_channel *oldchan, struct ast_channel *newchan)
1167 {
1168
1169         struct mbl_pvt *pvt = newchan->tech_pvt;
1170
1171         if (!pvt) {
1172                 ast_debug(1, "fixup failed, no pvt on newchan\n");
1173                 return -1;
1174         }
1175
1176         ast_mutex_lock(&pvt->lock);
1177         if (pvt->owner == oldchan)
1178                 pvt->owner = newchan;
1179         ast_mutex_unlock(&pvt->lock);
1180
1181         return 0;
1182
1183 }
1184
1185 static int mbl_devicestate(void *data)
1186 {
1187
1188         char *device;
1189         int res = AST_DEVICE_INVALID;
1190         struct mbl_pvt *pvt;
1191
1192         device = ast_strdupa(S_OR(data, ""));
1193
1194         ast_debug(1, "Checking device state for device %s\n", device);
1195
1196         AST_RWLIST_RDLOCK(&devices);
1197         AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
1198                 if (!strcmp(pvt->id, device))
1199                         break;
1200         }
1201         AST_RWLIST_UNLOCK(&devices);
1202
1203         if (!pvt)
1204                 return res;
1205
1206         ast_mutex_lock(&pvt->lock);
1207         if (pvt->connected) {
1208                 if (pvt->owner)
1209                         res = AST_DEVICE_INUSE;
1210                 else
1211                         res = AST_DEVICE_NOT_INUSE;
1212
1213                 if (!mbl_has_service(pvt))
1214                         res = AST_DEVICE_UNAVAILABLE;
1215         }
1216         ast_mutex_unlock(&pvt->lock);
1217
1218         return res;
1219
1220 }
1221
1222 /*
1223
1224         Callback helpers
1225
1226 */
1227
1228 /*
1229
1230         do_alignment_detection()
1231
1232         This routine attempts to detect where we get misaligned sco audio data from the bluetooth adaptor.
1233
1234         Its enabled by alignmentdetect=yes under the adapter entry in mobile.conf
1235
1236         Some adapters suffer a problem where occasionally they will byte shift the audio stream one byte to the right.
1237         The result is static or white noise on the inbound (from the adapter) leg of the call.
1238         This is characterised by a sudden jump in magnitude of the value of the 16 bit samples.
1239
1240         Here we look at the first 4 48 byte frames. We average the absolute values of each sample in the frame,
1241         then average the sum of the averages of frames 1, 2, and 3.
1242         Frame zero is usually zero.
1243         If the end result > 100, and it usually is if we have the problem, set a flag and compensate by shifting the bytes
1244         for each subsequent frame during the call.
1245
1246         If the result is <= 100 then clear the flag so we dont come back in here...
1247
1248         This seems to work OK....
1249
1250 */
1251
1252 static void do_alignment_detection(struct mbl_pvt *pvt, char *buf, int buflen)
1253 {
1254
1255         int i;
1256         short a, *s;
1257         char *p;
1258
1259         if (pvt->alignment_detection_triggered) {
1260                 for (i=buflen, p=buf+buflen-1; i>0; i--, p--)
1261                         *p = *(p-1);
1262                 *(p+1) = 0;
1263                 return;
1264         }
1265
1266         if (pvt->alignment_count < 4) {
1267                 s = (short *)buf;
1268                 for (i=0, a=0; i<buflen/2; i++) {
1269                         a += *s++;
1270                         a /= i+1;
1271                 }
1272                 pvt->alignment_samples[pvt->alignment_count++] = a;
1273                 return;
1274         }
1275
1276         ast_debug(1, "Alignment Detection result is [%-d %-d %-d %-d]\n", pvt->alignment_samples[0], pvt->alignment_samples[1], pvt->alignment_samples[2], pvt->alignment_samples[3]);
1277
1278         a = abs(pvt->alignment_samples[1]) + abs(pvt->alignment_samples[2]) + abs(pvt->alignment_samples[3]);
1279         a /= 3;
1280         if (a > 100) {
1281                 pvt->alignment_detection_triggered = 1;
1282                 ast_debug(1, "Alignment Detection Triggered.\n");
1283         } else
1284                 pvt->do_alignment_detection = 0;
1285
1286 }
1287
1288 static int mbl_queue_control(struct mbl_pvt *pvt, enum ast_control_frame_type control)
1289 {
1290         for (;;) {
1291                 if (pvt->owner) {
1292                         if (ast_channel_trylock(pvt->owner)) {
1293                                 DEADLOCK_AVOIDANCE(&pvt->lock);
1294                         } else {
1295                                 ast_queue_control(pvt->owner, control);
1296                                 ast_channel_unlock(pvt->owner);
1297                                 break;
1298                         }
1299                 } else
1300                         break;
1301         }
1302         return 0;
1303 }
1304
1305 static int mbl_queue_hangup(struct mbl_pvt *pvt)
1306 {
1307         for (;;) {
1308                 if (pvt->owner) {
1309                         if (ast_channel_trylock(pvt->owner)) {
1310                                 DEADLOCK_AVOIDANCE(&pvt->lock);
1311                         } else {
1312                                 ast_queue_hangup(pvt->owner);
1313                                 ast_channel_unlock(pvt->owner);
1314                                 break;
1315                         }
1316                 } else
1317                         break;
1318         }
1319         return 0;
1320 }
1321
1322 static int mbl_ast_hangup(struct mbl_pvt *pvt)
1323 {
1324         int res = 0;
1325         for (;;) {
1326                 if (pvt->owner) {
1327                         if (ast_channel_trylock(pvt->owner)) {
1328                                 DEADLOCK_AVOIDANCE(&pvt->lock);
1329                         } else {
1330                                 res = ast_hangup(pvt->owner);
1331                                 /* no need to unlock, ast_hangup() frees the
1332                                  * channel */
1333                                 break;
1334                         }
1335                 } else
1336                         break;
1337         }
1338         return res;
1339 }
1340
1341 /*!
1342  * \brief Check if a mobile device has service.
1343  * \param pvt a mbl_pvt struct
1344  * \retval 1 this device has service
1345  * \retval 0 no service
1346  *
1347  * \note This function will always indicate that service is available if the
1348  * given device does not support service indication.
1349  */
1350 static int mbl_has_service(struct mbl_pvt *pvt)
1351 {
1352
1353         if (pvt->type != MBL_TYPE_PHONE)
1354                 return 1;
1355
1356         if (!pvt->hfp->cind_map.service)
1357                 return 1;
1358
1359         if (pvt->hfp->cind_state[pvt->hfp->cind_map.service] == HFP_CIND_SERVICE_AVAILABLE)
1360                 return 1;
1361
1362         return 0;
1363 }
1364
1365 /*
1366
1367         rfcomm helpers
1368
1369 */
1370
1371 static int rfcomm_connect(bdaddr_t src, bdaddr_t dst, int remote_channel)
1372 {
1373
1374         struct sockaddr_rc addr;
1375         int s;
1376
1377         if ((s = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM)) < 0) {
1378                 ast_debug(1, "socket() failed (%d).\n", errno);
1379                 return -1;
1380         }
1381
1382         memset(&addr, 0, sizeof(addr));
1383         addr.rc_family = AF_BLUETOOTH;
1384         bacpy(&addr.rc_bdaddr, &src);
1385         addr.rc_channel = (uint8_t) 1;
1386         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
1387                 ast_debug(1, "bind() failed (%d).\n", errno);
1388                 close(s);
1389                 return -1;
1390         }
1391
1392         memset(&addr, 0, sizeof(addr));
1393         addr.rc_family = AF_BLUETOOTH;
1394         bacpy(&addr.rc_bdaddr, &dst);
1395         addr.rc_channel = remote_channel;
1396         if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
1397                 ast_debug(1, "connect() failed (%d).\n", errno);
1398                 close(s);
1399                 return -1;
1400         }
1401
1402         return s;
1403
1404 }
1405
1406 /*!
1407  * \brief Write to an rfcomm socket.
1408  * \param rsock the socket to write to
1409  * \param buf the null terminated buffer to write
1410  *
1411  * This function will write characters from buf.  The buffer must be null
1412  * terminated.
1413  *
1414  * \retval -1 error
1415  * \retval 0 success
1416  */
1417 static int rfcomm_write(int rsock, char *buf)
1418 {
1419         return rfcomm_write_full(rsock, buf, strlen(buf));
1420 }
1421
1422
1423 /*!
1424  * \brief Write to an rfcomm socket.
1425  * \param rsock the socket to write to
1426  * \param buf the buffer to write
1427  * \param count the number of characters from the buffer to write
1428  *
1429  * This function will write count characters from buf.  It will always write
1430  * count chars unless it encounters an error.
1431  *
1432  * \retval -1 error
1433  * \retval 0 success
1434  */
1435 static int rfcomm_write_full(int rsock, char *buf, size_t count)
1436 {
1437         char *p = buf;
1438         ssize_t out_count;
1439
1440         ast_debug(1, "rfcomm_write() (%d) [%.*s]\n", rsock, (int) count, buf);
1441         while (count > 0) {
1442                 if ((out_count = write(rsock, p, count)) == -1) {
1443                         ast_debug(1, "rfcomm_write() error [%d]\n", errno);
1444                         return -1;
1445                 }
1446                 count -= out_count;
1447                 p += out_count;
1448         }
1449
1450         return 0;
1451 }
1452
1453 /*!
1454  * \brief Wait for activity on an rfcomm socket.
1455  * \param rsock the socket to watch
1456  * \param ms a pointer to an int containing a timeout in ms
1457  * \return zero on timeout and the socket fd (non-zero) otherwise
1458  * \retval 0 timeout
1459  */
1460 static int rfcomm_wait(int rsock, int *ms)
1461 {
1462         int exception, outfd;
1463         outfd = ast_waitfor_n_fd(&rsock, 1, ms, &exception);
1464         if (outfd < 0)
1465                 outfd = 0;
1466
1467         return outfd;
1468 }
1469
1470 #ifdef RFCOMM_READ_DEBUG
1471 #define rfcomm_read_debug(c) __rfcomm_read_debug(c)
1472 static void __rfcomm_read_debug(char c)
1473 {
1474         if (c == '\r')
1475                 ast_debug(2, "rfcomm_read: \\r\n");
1476         else if (c == '\n')
1477                 ast_debug(2, "rfcomm_read: \\n\n");
1478         else
1479                 ast_debug(2, "rfcomm_read: %c\n", c);
1480 }
1481 #else
1482 #define rfcomm_read_debug(c)
1483 #endif
1484
1485 /*!
1486  * \brief Append the given character to the given buffer and increase the
1487  * in_count.
1488  */
1489 static void inline rfcomm_append_buf(char **buf, size_t count, size_t *in_count, char c)
1490 {
1491         if (*in_count < count) {
1492                 (*in_count)++;
1493                 *(*buf)++ = c;
1494         }
1495 }
1496
1497 /*!
1498  * \brief Read a character from the given stream and check if it matches what
1499  * we expected.
1500  */
1501 static int rfcomm_read_and_expect_char(int rsock, char *result, char expected)
1502 {
1503         int res;
1504         char c;
1505
1506         if (!result)
1507                 result = &c;
1508
1509         if ((res = read(rsock, result, 1)) < 1) {
1510                 return res;
1511         }
1512         rfcomm_read_debug(*result);
1513
1514         if (*result != expected) {
1515                 return -2;
1516         }
1517
1518         return 1;
1519 }
1520
1521 /*!
1522  * \brief Read a character from the given stream and append it to the given
1523  * buffer if it matches the expected character.
1524  */
1525 static int rfcomm_read_and_append_char(int rsock, char **buf, size_t count, size_t *in_count, char *result, char expected)
1526 {
1527         int res;
1528         char c;
1529
1530         if (!result)
1531                 result = &c;
1532
1533         if ((res = rfcomm_read_and_expect_char(rsock, result, expected)) < 1) {
1534                 return res;
1535         }
1536
1537         rfcomm_append_buf(buf, count, in_count, *result);
1538         return 1;
1539 }
1540
1541 /*!
1542  * \brief Read until '\r\n'.
1543  * This function consumes the '\r\n' but does not add it to buf.
1544  */
1545 static int rfcomm_read_until_crlf(int rsock, char **buf, size_t count, size_t *in_count)
1546 {
1547         int res;
1548         char c;
1549
1550         while ((res = read(rsock, &c, 1)) == 1) {
1551                 rfcomm_read_debug(c);
1552                 if (c == '\r') {
1553                         if ((res = rfcomm_read_and_expect_char(rsock, &c, '\n')) == 1) {
1554                                 break;
1555                         } else if (res == -2) {
1556                                 rfcomm_append_buf(buf, count, in_count, '\r');
1557                         } else {
1558                                 rfcomm_append_buf(buf, count, in_count, '\r');
1559                                 break;
1560                         }
1561                 }
1562
1563                 rfcomm_append_buf(buf, count, in_count, c);
1564         }
1565         return res;
1566 }
1567
1568 /*!
1569  * \brief Read the remainder of an AT SMS prompt.
1570  * \note the entire parsed string is '\r\n> '
1571  *
1572  * By the time this function is executed, only a ' ' is left to read.
1573  */
1574 static int rfcomm_read_sms_prompt(int rsock, char **buf, size_t count, size_t *in_count)
1575 {
1576         int res;
1577         if ((res = rfcomm_read_and_append_char(rsock, buf, count, in_count, NULL, ' ')) < 1)
1578                goto e_return;
1579
1580         return 1;
1581
1582 e_return:
1583         ast_log(LOG_ERROR, "error parsing SMS prompt on rfcomm socket\n");
1584         return res;
1585 }
1586
1587 /*!
1588  * \brief Read until a \r\nOK\r\n message.
1589  */
1590 static int rfcomm_read_until_ok(int rsock, char **buf, size_t count, size_t *in_count)
1591 {
1592         int res;
1593         char c;
1594
1595         /* here, we read until finding a \r\n, then we read one character at a
1596          * time looking for the string '\r\nOK\r\n'.  If we only find a partial
1597          * match, we place that in the buffer and try again. */
1598
1599         for (;;) {
1600                 if ((res = rfcomm_read_until_crlf(rsock, buf, count, in_count)) != 1) {
1601                         break;
1602                 }
1603
1604                 rfcomm_append_buf(buf, count, in_count, '\r');
1605                 rfcomm_append_buf(buf, count, in_count, '\n');
1606
1607                 if ((res = rfcomm_read_and_expect_char(rsock, &c, '\r')) != 1) {
1608                         if (res != -2) {
1609                                 break;
1610                         }
1611
1612                         rfcomm_append_buf(buf, count, in_count, c);
1613                         continue;
1614                 }
1615
1616                 if ((res = rfcomm_read_and_expect_char(rsock, &c, '\n')) != 1) {
1617                         if (res != -2) {
1618                                 break;
1619                         }
1620
1621                         rfcomm_append_buf(buf, count, in_count, '\r');
1622                         rfcomm_append_buf(buf, count, in_count, c);
1623                         continue;
1624                 }
1625                 if ((res = rfcomm_read_and_expect_char(rsock, &c, 'O')) != 1) {
1626                         if (res != -2) {
1627                                 break;
1628                         }
1629
1630                         rfcomm_append_buf(buf, count, in_count, '\r');
1631                         rfcomm_append_buf(buf, count, in_count, '\n');
1632                         rfcomm_append_buf(buf, count, in_count, c);
1633                         continue;
1634                 }
1635
1636                 if ((res = rfcomm_read_and_expect_char(rsock, &c, 'K')) != 1) {
1637                         if (res != -2) {
1638                                 break;
1639                         }
1640
1641                         rfcomm_append_buf(buf, count, in_count, '\r');
1642                         rfcomm_append_buf(buf, count, in_count, '\n');
1643                         rfcomm_append_buf(buf, count, in_count, 'O');
1644                         rfcomm_append_buf(buf, count, in_count, c);
1645                         continue;
1646                 }
1647
1648                 if ((res = rfcomm_read_and_expect_char(rsock, &c, '\r')) != 1) {
1649                         if (res != -2) {
1650                                 break;
1651                         }
1652
1653                         rfcomm_append_buf(buf, count, in_count, '\r');
1654                         rfcomm_append_buf(buf, count, in_count, '\n');
1655                         rfcomm_append_buf(buf, count, in_count, 'O');
1656                         rfcomm_append_buf(buf, count, in_count, 'K');
1657                         rfcomm_append_buf(buf, count, in_count, c);
1658                         continue;
1659                 }
1660
1661                 if ((res = rfcomm_read_and_expect_char(rsock, &c, '\n')) != 1) {
1662                         if (res != -2) {
1663                                 break;
1664                         }
1665
1666                         rfcomm_append_buf(buf, count, in_count, '\r');
1667                         rfcomm_append_buf(buf, count, in_count, '\n');
1668                         rfcomm_append_buf(buf, count, in_count, 'O');
1669                         rfcomm_append_buf(buf, count, in_count, 'K');
1670                         rfcomm_append_buf(buf, count, in_count, '\r');
1671                         rfcomm_append_buf(buf, count, in_count, c);
1672                         continue;
1673                 }
1674
1675                 /* we have successfully parsed a '\r\nOK\r\n' string */
1676                 return 1;
1677         }
1678
1679         return res;
1680 }
1681
1682
1683 /*!
1684  * \brief Read the remainder of a +CMGR message.
1685  * \note the entire parsed string is '+CMGR: ...\r\n...\r\n...\r\n...\r\nOK\r\n'
1686  */
1687 static int rfcomm_read_cmgr(int rsock, char **buf, size_t count, size_t *in_count)
1688 {
1689         int res;
1690
1691         /* append the \r\n that was stripped by the calling function */
1692         rfcomm_append_buf(buf, count, in_count, '\r');
1693         rfcomm_append_buf(buf, count, in_count, '\n');
1694
1695         if ((res = rfcomm_read_until_ok(rsock, buf, count, in_count)) != 1) {
1696                 ast_log(LOG_ERROR, "error reading +CMGR message on rfcomm socket\n");
1697         }
1698
1699         return res;
1700 }
1701
1702 /*!
1703  * \brief Read and AT result code.
1704  * \note the entire parsed string is '\r\n<result code>\r\n'
1705  */
1706 static int rfcomm_read_result(int rsock, char **buf, size_t count, size_t *in_count)
1707 {
1708         int res;
1709         char c;
1710
1711         if ((res = rfcomm_read_and_expect_char(rsock, &c, '\n')) < 1) {
1712                 goto e_return;
1713         }
1714
1715         if ((res = rfcomm_read_and_append_char(rsock, buf, count, in_count, &c, '>')) == 1) {
1716                 return rfcomm_read_sms_prompt(rsock, buf, count, in_count);
1717         } else if (res != -2) {
1718                 goto e_return;
1719         }
1720
1721         rfcomm_append_buf(buf, count, in_count, c);
1722         res = rfcomm_read_until_crlf(rsock, buf, count, in_count);
1723
1724         if (res != 1)
1725                 return res;
1726
1727         /* check for CMGR, which contains an embedded \r\n pairs terminated by
1728          * an \r\nOK\r\n message */
1729         if (*in_count >= 5 && !strncmp(*buf - *in_count, "+CMGR", 5)) {
1730                 return rfcomm_read_cmgr(rsock, buf, count, in_count);
1731         }
1732
1733         return 1;
1734
1735 e_return:
1736         ast_log(LOG_ERROR, "error parsing AT result on rfcomm socket");
1737         return res;
1738 }
1739
1740 /*!
1741  * \brief Read the remainder of an AT command.
1742  * \note the entire parsed string is '<at command>\r'
1743  */
1744 static int rfcomm_read_command(int rsock, char **buf, size_t count, size_t *in_count)
1745 {
1746         int res;
1747         char c;
1748
1749         while ((res = read(rsock, &c, 1)) == 1) {
1750                 rfcomm_read_debug(c);
1751                 /* stop when we get to '\r' */
1752                 if (c == '\r')
1753                         break;
1754
1755                 rfcomm_append_buf(buf, count, in_count, c);
1756         }
1757         return res;
1758 }
1759
1760 /*!
1761  * \brief Read one Hayes AT message from an rfcomm socket.
1762  * \param rsock the rfcomm socket to read from
1763  * \param buf the buffer to store the result in
1764  * \param count the size of the buffer or the maximum number of characters to read
1765  *
1766  * Here we need to read complete Hayes AT messages.  The AT message formats we
1767  * support are listed below.
1768  *
1769  * \verbatim
1770  * \r\n<result code>\r\n
1771  * <at command>\r
1772  * \r\n> 
1773  * \endverbatim
1774  *
1775  * These formats correspond to AT result codes, AT commands, and the AT SMS
1776  * prompt respectively.  When messages are read the leading and trailing '\r'
1777  * and '\n' characters are discarded.  If the given buffer is not large enough
1778  * to hold the response, what does not fit in the buffer will be dropped.
1779  *
1780  * \note The rfcomm connection to the device is asynchronous, so there is no
1781  * guarantee that responses will be returned in a single read() call. We handle
1782  * this by blocking until we can read an entire response.
1783  *
1784  * \retval 0 end of file
1785  * \retval -1 read error
1786  * \retval -2 parse error
1787  * \retval other the number of characters added to buf
1788  */
1789 static ssize_t rfcomm_read(int rsock, char *buf, size_t count)
1790 {
1791         ssize_t res;
1792         size_t in_count = 0;
1793         char c;
1794
1795         if ((res = rfcomm_read_and_expect_char(rsock, &c, '\r')) == 1) {
1796                 res = rfcomm_read_result(rsock, &buf, count, &in_count);
1797         } else if (res == -2) {
1798                 rfcomm_append_buf(&buf, count, &in_count, c);
1799                 res = rfcomm_read_command(rsock, &buf, count, &in_count);
1800         }
1801
1802         if (res < 1)
1803                 return res;
1804         else
1805                 return in_count;
1806 }
1807
1808 /*
1809
1810         sco helpers and callbacks
1811
1812 */
1813
1814 static int sco_connect(bdaddr_t src, bdaddr_t dst)
1815 {
1816
1817         struct sockaddr_sco addr;
1818         int s;
1819
1820         if ((s = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO)) < 0) {
1821                 ast_debug(1, "socket() failed (%d).\n", errno);
1822                 return -1;
1823         }
1824
1825 /* XXX this does not work with the do_sco_listen() thread (which also bind()s
1826  * to this address).  Also I am not sure if it is necessary. */
1827 #if 0
1828         memset(&addr, 0, sizeof(addr));
1829         addr.sco_family = AF_BLUETOOTH;
1830         bacpy(&addr.sco_bdaddr, &src);
1831         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
1832                 ast_debug(1, "bind() failed (%d).\n", errno);
1833                 close(s);
1834                 return -1;
1835         }
1836 #endif
1837
1838         memset(&addr, 0, sizeof(addr));
1839         addr.sco_family = AF_BLUETOOTH;
1840         bacpy(&addr.sco_bdaddr, &dst);
1841
1842         if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
1843                 ast_debug(1, "sco connect() failed (%d).\n", errno);
1844                 close(s);
1845                 return -1;
1846         }
1847
1848         return s;
1849
1850 }
1851
1852 static int sco_write(int s, char *buf, int len)
1853 {
1854
1855         int r;
1856
1857         if (s == -1) {
1858                 ast_debug(3, "sco_write() not ready\n");
1859                 return 0;
1860         }
1861
1862         ast_debug(3, "sco_write()\n");
1863
1864         r = write(s, buf, len);
1865         if (r == -1) {
1866                 ast_debug(3, "sco write error %d\n", errno);
1867                 return 0;
1868         }
1869
1870         return 1;
1871
1872 }
1873
1874 /*!
1875  * \brief Accept SCO connections.
1876  * This function is an ast_io callback function used to accept incoming sco
1877  * audio connections.
1878  */
1879 static int sco_accept(int *id, int fd, short events, void *data)
1880 {
1881         struct adapter_pvt *adapter = (struct adapter_pvt *) data;
1882         struct sockaddr_sco addr;
1883         socklen_t addrlen;
1884         struct mbl_pvt *pvt;
1885         socklen_t len;
1886         char saddr[18];
1887         struct sco_options so;
1888         int sock;
1889
1890         addrlen = sizeof(struct sockaddr_sco);
1891         if ((sock = accept(fd, (struct sockaddr *)&addr, &addrlen)) == -1) {
1892                 ast_log(LOG_ERROR, "error accepting audio connection on adapter %s\n", adapter->id);
1893                 return 0;
1894         }
1895
1896         len = sizeof(so);
1897         getsockopt(sock, SOL_SCO, SCO_OPTIONS, &so, &len);
1898
1899         ba2str(&addr.sco_bdaddr, saddr);
1900         ast_debug(1, "Incoming Audio Connection from device %s MTU is %d\n", saddr, so.mtu);
1901
1902         /* figure out which device this sco connection belongs to */
1903         pvt = NULL;
1904         AST_RWLIST_RDLOCK(&devices);
1905         AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
1906                 if (!bacmp(&pvt->addr, &addr.sco_bdaddr))
1907                         break;
1908         }
1909         AST_RWLIST_UNLOCK(&devices);
1910         if (!pvt) {
1911                 ast_log(LOG_WARNING, "could not find device for incoming audio connection\n");
1912                 close(sock);
1913                 return 1;
1914         }
1915
1916         ast_mutex_lock(&pvt->lock);
1917         if (pvt->sco_socket != -1) {
1918                 close(pvt->sco_socket);
1919                 pvt->sco_socket = -1;
1920         }
1921
1922         pvt->sco_socket = sock;
1923         if (pvt->owner) {
1924                 ast_channel_set_fd(pvt->owner, 0, sock);
1925         } else {
1926                 ast_debug(1, "incoming audio connection for pvt without owner\n");
1927         }
1928
1929         ast_mutex_unlock(&pvt->lock);
1930
1931         return 1;
1932 }
1933
1934 /*!
1935  * \brief Bind an SCO listener socket for the given adapter.
1936  * \param adapter an adapter_pvt
1937  * \return -1 on error, non zero on success
1938  */
1939 static int sco_bind(struct adapter_pvt *adapter)
1940 {
1941         struct sockaddr_sco addr;
1942         int opt = 1;
1943
1944         if ((adapter->sco_socket = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO)) < 0) {
1945                 ast_log(LOG_ERROR, "Unable to create sco listener socket for adapter %s.\n", adapter->id);
1946                 goto e_return;
1947         }
1948
1949         memset(&addr, 0, sizeof(addr));
1950         addr.sco_family = AF_BLUETOOTH;
1951         bacpy(&addr.sco_bdaddr, &adapter->addr);
1952         if (bind(adapter->sco_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
1953                 ast_log(LOG_ERROR, "Unable to bind sco listener socket. (%d)\n", errno);
1954                 goto e_close_socket;
1955         }
1956         if (setsockopt(adapter->sco_socket, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) == -1) {
1957                 ast_log(LOG_ERROR, "Unable to setsockopt sco listener socket.\n");
1958                 goto e_close_socket;
1959         }
1960         if (listen(adapter->sco_socket, 5) < 0) {
1961                 ast_log(LOG_ERROR, "Unable to listen sco listener socket.\n");
1962                 goto e_close_socket;
1963         }
1964
1965         return adapter->sco_socket;
1966
1967 e_close_socket:
1968         close(adapter->sco_socket);
1969         adapter->sco_socket = -1;
1970 e_return:
1971         return -1;
1972 }
1973
1974
1975 /*
1976  * Hayes AT command helpers.
1977  */
1978
1979 /*!
1980  * \brief Match the given buffer with the given prefix.
1981  * \param buf the buffer to match
1982  * \param prefix the prefix to match
1983  */
1984 static int at_match_prefix(char *buf, char *prefix)
1985 {
1986         return !strncmp(buf, prefix, strlen(prefix));
1987 }
1988
1989 /*!
1990  * \brief Read an AT message and clasify it.
1991  * \param rsock an rfcomm socket
1992  * \param buf the buffer to store the result in
1993  * \param count the size of the buffer or the maximum number of characters to read
1994  * \return the type of message received, in addition buf will contain the
1995  * message received and will be null terminated
1996  * \see at_read()
1997  */
1998 static at_message_t at_read_full(int rsock, char *buf, size_t count)
1999 {
2000         ssize_t s;
2001         if ((s = rfcomm_read(rsock, buf, count - 1)) < 1)
2002                 return s;
2003         buf[s] = '\0';
2004
2005         if (!strcmp("OK", buf)) {
2006                 return AT_OK;
2007         } else if (!strcmp("ERROR", buf)) {
2008                 return AT_ERROR;
2009         } else if (!strcmp("RING", buf)) {
2010                 return AT_RING;
2011         } else if (!strcmp("AT+CKPD=200", buf)) {
2012                 return AT_CKPD;
2013         } else if (!strcmp("> ", buf)) {
2014                 return AT_SMS_PROMPT;
2015         } else if (at_match_prefix(buf, "+CMTI:")) {
2016                 return AT_CMTI;
2017         } else if (at_match_prefix(buf, "+CIEV:")) {
2018                 return AT_CIEV;
2019         } else if (at_match_prefix(buf, "+BRSF:")) {
2020                 return AT_BRSF;
2021         } else if (at_match_prefix(buf, "+CIND:")) {
2022                 return AT_CIND;
2023         } else if (at_match_prefix(buf, "+CLIP:")) {
2024                 return AT_CLIP;
2025         } else if (at_match_prefix(buf, "+CMGR:")) {
2026                 return AT_CMGR;
2027         } else if (at_match_prefix(buf, "+VGM:")) {
2028                 return AT_VGM;
2029         } else if (at_match_prefix(buf, "+VGS:")) {
2030                 return AT_VGS;
2031         } else if (at_match_prefix(buf, "+CMS ERROR:")) {
2032                 return AT_CMS_ERROR;
2033         } else if (at_match_prefix(buf, "AT+VGM=")) {
2034                 return AT_VGM;
2035         } else if (at_match_prefix(buf, "AT+VGS=")) {
2036                 return AT_VGS;
2037         } else if (at_match_prefix(buf, "+CUSD:")) {
2038                 return AT_CUSD;
2039         } else {
2040                 return AT_UNKNOWN;
2041         }
2042 }
2043
2044 /*!
2045  * \brief Get the string representation of the given AT message.
2046  * \param msg the message to process
2047  * \return a string describing the given message
2048  */
2049 static inline const char *at_msg2str(at_message_t msg)
2050 {
2051         switch (msg) {
2052         /* errors */
2053         case AT_PARSE_ERROR:
2054                 return "PARSE ERROR";
2055         case AT_READ_ERROR:
2056                 return "READ ERROR";
2057         default:
2058         case AT_UNKNOWN:
2059                 return "UNKNOWN";
2060         /* at responses */
2061         case AT_OK:
2062                 return "OK";
2063         case AT_ERROR:
2064                 return "ERROR";
2065         case AT_RING:
2066                 return "RING";
2067         case AT_BRSF:
2068                 return "AT+BRSF";
2069         case AT_CIND:
2070                 return "AT+CIND";
2071         case AT_CIEV:
2072                 return "AT+CIEV";
2073         case AT_CLIP:
2074                 return "AT+CLIP";
2075         case AT_CMTI:
2076                 return "AT+CMTI";
2077         case AT_CMGR:
2078                 return "AT+CMGR";
2079         case AT_SMS_PROMPT:
2080                 return "SMS PROMPT";
2081         case AT_CMS_ERROR:
2082                 return "+CMS ERROR";
2083         /* at commands */
2084         case AT_A:
2085                 return "ATA";
2086         case AT_D:
2087                 return "ATD";
2088         case AT_CHUP:
2089                 return "AT+CHUP";
2090         case AT_CKPD:
2091                 return "AT+CKPD";
2092         case AT_CMGS:
2093                 return "AT+CMGS";
2094         case AT_VGM:
2095                 return "AT+VGM";
2096         case AT_VGS:
2097                 return "AT+VGS";
2098         case AT_VTS:
2099                 return "AT+VTS";
2100         case AT_CMGF:
2101                 return "AT+CMGF";
2102         case AT_CNMI:
2103                 return "AT+CNMI";
2104         case AT_CMER:
2105                 return "AT+CMER";
2106         case AT_CIND_TEST:
2107                 return "AT+CIND=?";
2108         case AT_CUSD:
2109                 return "AT+CUSD";
2110         }
2111 }
2112
2113
2114 /*
2115  * bluetooth handsfree profile helpers
2116  */
2117
2118 /*!
2119  * \brief Parse a CIEV event.
2120  * \param hfp an hfp_pvt struct
2121  * \param buf the buffer to parse (null terminated)
2122  * \param value a pointer to an int to store the event value in (can be NULL)
2123  * \return 0 on error (parse error, or unknown event) or a HFP_CIND_* value on
2124  * success
2125  */
2126 static int hfp_parse_ciev(struct hfp_pvt *hfp, char *buf, int *value)
2127 {
2128         int i, v;
2129         if (!value)
2130                 value = &v;
2131
2132         if (!sscanf(buf, "+CIEV: %d,%d", &i, value)) {
2133                 ast_debug(2, "[%s] error parsing CIEV event '%s'\n", hfp->owner->id, buf);
2134                 return HFP_CIND_NONE;
2135         }
2136
2137         if (i >= sizeof(hfp->cind_state)) {
2138                 ast_debug(2, "[%s] CIEV event index too high (%s)\n", hfp->owner->id, buf);
2139                 return HFP_CIND_NONE;
2140         }
2141
2142         hfp->cind_state[i] = *value;
2143         return hfp->cind_index[i];
2144 }
2145
2146 /*!
2147  * \brief Parse a CLIP event.
2148  * \param hfp an hfp_pvt struct
2149  * \param buf the buffer to parse (null terminated)
2150  * @note buf will be modified when the CID string is parsed
2151  * \return NULL on error (parse error) or a pointer to the caller id
2152  * information in buf
2153  */
2154 static char *hfp_parse_clip(struct hfp_pvt *hfp, char *buf)
2155 {
2156         int i, state;
2157         char *clip = NULL;
2158         size_t s;
2159
2160         /* parse clip info in the following format:
2161          * +CLIP: "123456789",128,...
2162          */
2163         state = 0;
2164         s = strlen(buf);
2165         for (i = 0; i < s && state != 3; i++) {
2166                 switch (state) {
2167                 case 0: /* search for start of the number (") */
2168                         if (buf[i] == '"') {
2169                                 state++;
2170                         }
2171                         break;
2172                 case 1: /* mark the number */
2173                         clip = &buf[i];
2174                         state++;
2175                         /* fall through */
2176                 case 2: /* search for the end of the number (") */
2177                         if (buf[i] == '"') {
2178                                 buf[i] = '\0';
2179                                 state++;
2180                         }
2181                         break;
2182                 }
2183         }
2184
2185         if (state != 3) {
2186                 return NULL;
2187         }
2188
2189         return clip;
2190 }
2191
2192 /*!
2193  * \brief Parse a CMTI notification.
2194  * \param hfp an hfp_pvt struct
2195  * \param buf the buffer to parse (null terminated)
2196  * @note buf will be modified when the CMTI message is parsed
2197  * \return -1 on error (parse error) or the index of the new sms message
2198  */
2199 static int hfp_parse_cmti(struct hfp_pvt *hfp, char *buf)
2200 {
2201         int index = -1;
2202
2203         /* parse cmti info in the following format:
2204          * +CMTI: <mem>,<index> 
2205          */
2206         if (!sscanf(buf, "+CMTI: %*[^,],%d", &index)) {
2207                 ast_debug(2, "[%s] error parsing CMTI event '%s'\n", hfp->owner->id, buf);
2208                 return -1;
2209         }
2210
2211         return index;
2212 }
2213
2214 /*!
2215  * \brief Parse a CMGR message.
2216  * \param hfp an hfp_pvt struct
2217  * \param buf the buffer to parse (null terminated)
2218  * \param from_number a pointer to a char pointer which will store the from
2219  * number
2220  * \param text a pointer to a char pointer which will store the message text
2221  * @note buf will be modified when the CMGR message is parsed
2222  * \retval -1 parse error
2223  * \retval 0 success
2224  */
2225 static int hfp_parse_cmgr(struct hfp_pvt *hfp, char *buf, char **from_number, char **text)
2226 {
2227         int i, state;
2228         size_t s;
2229
2230         /* parse cmgr info in the following format:
2231          * +CMGR: <msg status>,"+123456789",...\r\n
2232          * <message text>
2233          */
2234         state = 0;
2235         s = strlen(buf);
2236         for (i = 0; i < s && s != 6; i++) {
2237                 switch (state) {
2238                 case 0: /* search for start of the number section (,) */
2239                         if (buf[i] == ',') {
2240                                 state++;
2241                         }
2242                         break;
2243                 case 1: /* find the opening quote (") */
2244                         if (buf[i] == '"') {
2245                                 state++;
2246                         }
2247                 case 2: /* mark the start of the number */
2248                         if (from_number) {
2249                                 *from_number = &buf[i];
2250                                 state++;
2251                         }
2252                         /* fall through */
2253                 case 3: /* search for the end of the number (") */
2254                         if (buf[i] == '"') {
2255                                 buf[i] = '\0';
2256                                 state++;
2257                         }
2258                         break;
2259                 case 4: /* search for the start of the message text (\n) */
2260                         if (buf[i] == '\n') {
2261                                 state++;
2262                         }
2263                         break;
2264                 case 5: /* mark the start of the message text */
2265                         if (text) {
2266                                 *text = &buf[i];
2267                                 state++;
2268                         }
2269                         break;
2270                 }
2271         }
2272
2273         if (state != 6) {
2274                 return -1;
2275         }
2276
2277         return 0;
2278 }
2279
2280 /*!
2281  * \brief Parse a CUSD answer.
2282  * \param hfp an hfp_pvt struct
2283  * \param buf the buffer to parse (null terminated)
2284  * @note buf will be modified when the CUSD string is parsed
2285  * \return NULL on error (parse error) or a pointer to the cusd message
2286  * information in buf
2287  */
2288 static char *hfp_parse_cusd(struct hfp_pvt *hfp, char *buf)
2289 {
2290         int i, state, message_start, message_end;
2291         char *cusd;
2292         size_t s;
2293
2294         /* parse cusd message in the following format:
2295          * +CUSD: 0,"100,00 EURO, valid till 01.01.2010, you are using tariff "Mega Tariff". More informations *111#."
2296          */
2297         state = 0;
2298         message_start = 0;
2299         message_end = 0;
2300         s = strlen(buf);
2301
2302         /* Find the start of the message (") */
2303         for (i = 0; i < s; i++) {
2304                 if (buf[i] == '"') {
2305                         message_start = i + 1;
2306                         break;
2307                 }
2308         }
2309
2310         if (message_start == 0 || message_start >= s) {
2311                 return NULL;
2312         }
2313
2314         /* Find the end of the message (") */
2315         for (i = s; i > 0; i--) {
2316                 if (buf[i] == '"') {
2317                         message_end = i;
2318                         break;
2319                 }
2320         }
2321
2322         if (message_end == 0) {
2323                 return NULL;
2324         }
2325
2326         if (message_start >= message_end) {
2327                 return NULL;
2328         }
2329
2330         cusd = &buf[message_start];
2331         buf[message_end] = '\0';
2332
2333         return cusd;
2334 }
2335
2336 /*!
2337  * \brief Convert a hfp_hf struct to a BRSF int.
2338  * \param hf an hfp_hf brsf object
2339  * \return an integer representing the given brsf struct
2340  */
2341 static int hfp_brsf2int(struct hfp_hf *hf)
2342 {
2343         int brsf = 0;
2344
2345         brsf |= hf->ecnr ? HFP_HF_ECNR : 0;
2346         brsf |= hf->cw ? HFP_HF_CW : 0;
2347         brsf |= hf->cid ? HFP_HF_CID : 0;
2348         brsf |= hf->voice ? HFP_HF_VOICE : 0;
2349         brsf |= hf->volume ? HFP_HF_VOLUME : 0;
2350         brsf |= hf->status ? HFP_HF_STATUS : 0;
2351         brsf |= hf->control ? HFP_HF_CONTROL : 0;
2352
2353         return brsf;
2354 }
2355
2356 /*!
2357  * \brief Convert a BRSF int to an hfp_ag struct.
2358  * \param brsf a brsf integer
2359  * \param ag a AG (hfp_ag) brsf object
2360  * \return a pointer to the given hfp_ag object populated with the values from
2361  * the given brsf integer
2362  */
2363 static struct hfp_ag *hfp_int2brsf(int brsf, struct hfp_ag *ag)
2364 {
2365         ag->cw = brsf & HFP_AG_CW ? 1 : 0;
2366         ag->ecnr = brsf & HFP_AG_ECNR ? 1 : 0;
2367         ag->voice = brsf & HFP_AG_VOICE ? 1 : 0;
2368         ag->ring = brsf & HFP_AG_RING ? 1 : 0;
2369         ag->tag = brsf & HFP_AG_TAG ? 1 : 0;
2370         ag->reject = brsf & HFP_AG_REJECT ? 1 : 0;
2371         ag->status = brsf & HFP_AG_STATUS ? 1 : 0;
2372         ag->control = brsf & HFP_AG_CONTROL ? 1 : 0;
2373         ag->errors = brsf & HFP_AG_ERRORS ? 1 : 0;
2374
2375         return ag;
2376 }
2377
2378
2379 /*!
2380  * \brief Send a BRSF request.
2381  * \param hfp an hfp_pvt struct
2382  * \param brsf an hfp_hf brsf struct
2383  *
2384  * \retval 0 on success
2385  * \retval -1 on error
2386  */
2387 static int hfp_send_brsf(struct hfp_pvt *hfp, struct hfp_hf *brsf)
2388 {
2389         char cmd[32];
2390         snprintf(cmd, sizeof(cmd), "AT+BRSF=%d\r", hfp_brsf2int(brsf));
2391         return rfcomm_write(hfp->rsock, cmd);
2392 }
2393
2394 /*!
2395  * \brief Send the CIND read command.
2396  * \param hfp an hfp_pvt struct
2397  */
2398 static int hfp_send_cind(struct hfp_pvt *hfp)
2399 {
2400         return rfcomm_write(hfp->rsock, "AT+CIND?\r");
2401 }
2402
2403 /*!
2404  * \brief Send the CIND test command.
2405  * \param hfp an hfp_pvt struct
2406  */
2407 static int hfp_send_cind_test(struct hfp_pvt *hfp)
2408 {
2409         return rfcomm_write(hfp->rsock, "AT+CIND=?\r");
2410 }
2411
2412 /*!
2413  * \brief Enable or disable indicator events reporting.
2414  * \param hfp an hfp_pvt struct
2415  * \param status enable or disable events reporting (should be 1 or 0)
2416  */
2417 static int hfp_send_cmer(struct hfp_pvt *hfp, int status)
2418 {
2419         char cmd[32];
2420         snprintf(cmd, sizeof(cmd), "AT+CMER=3,0,0,%d\r", status ? 1 : 0);
2421         return rfcomm_write(hfp->rsock, cmd);
2422 }
2423
2424 /*!
2425  * \brief Send the current speaker gain level.
2426  * \param hfp an hfp_pvt struct
2427  * \param value the value to send (must be between 0 and 15)
2428  */
2429 static int hfp_send_vgs(struct hfp_pvt *hfp, int value)
2430 {
2431         char cmd[32];
2432         snprintf(cmd, sizeof(cmd), "AT+VGS=%d\r", value);
2433         return rfcomm_write(hfp->rsock, cmd);
2434 }
2435
2436 #if 0
2437 /*!
2438  * \brief Send the current microphone gain level.
2439  * \param hfp an hfp_pvt struct
2440  * \param value the value to send (must be between 0 and 15)
2441  */
2442 static int hfp_send_vgm(struct hfp_pvt *hfp, int value)
2443 {
2444         char cmd[32];
2445         snprintf(cmd, sizeof(cmd), "AT+VGM=%d\r", value);
2446         return rfcomm_write(hfp->rsock, cmd);
2447 }
2448 #endif
2449
2450 /*!
2451  * \brief Enable or disable calling line identification.
2452  * \param hfp an hfp_pvt struct
2453  * \param status enable or disable calling line identification (should be 1 or
2454  * 0)
2455  */
2456 static int hfp_send_clip(struct hfp_pvt *hfp, int status)
2457 {
2458         char cmd[32];
2459         snprintf(cmd, sizeof(cmd), "AT+CLIP=%d\r", status ? 1 : 0);
2460         return rfcomm_write(hfp->rsock, cmd);
2461 }
2462
2463 /*!
2464  * \brief Send a DTMF command.
2465  * \param hfp an hfp_pvt struct
2466  * \param digit the dtmf digit to send
2467  * \return the result of rfcomm_write() or -1 on an invalid digit being sent
2468  */
2469 static int hfp_send_dtmf(struct hfp_pvt *hfp, char digit)
2470 {
2471         char cmd[10];
2472
2473         switch(digit) {
2474         case '0':
2475         case '1':
2476         case '2':
2477         case '3':
2478         case '4':
2479         case '5':
2480         case '6':
2481         case '7':
2482         case '8':
2483         case '9':
2484         case '*':
2485         case '#':
2486                 snprintf(cmd, sizeof(cmd), "AT+VTS=%c\r", digit);
2487                 return rfcomm_write(hfp->rsock, cmd);
2488         default:
2489                 return -1;
2490         }
2491 }
2492
2493 /*!
2494  * \brief Set the SMS mode.
2495  * \param hfp an hfp_pvt struct
2496  * \param mode the sms mode (0 = PDU, 1 = Text)
2497  */
2498 static int hfp_send_cmgf(struct hfp_pvt *hfp, int mode)
2499 {
2500         char cmd[32];
2501         snprintf(cmd, sizeof(cmd), "AT+CMGF=%d\r", mode);
2502         return rfcomm_write(hfp->rsock, cmd);
2503 }
2504
2505 /*!
2506  * \brief Setup SMS new message indication.
2507  * \param hfp an hfp_pvt struct
2508  */
2509 static int hfp_send_cnmi(struct hfp_pvt *hfp)
2510 {
2511         return rfcomm_write(hfp->rsock, "AT+CNMI=2,1,0,0,0\r");
2512 }
2513
2514 /*!
2515  * \brief Read an SMS message.
2516  * \param hfp an hfp_pvt struct
2517  * \param index the location of the requested message
2518  */
2519 static int hfp_send_cmgr(struct hfp_pvt *hfp, int index)
2520 {
2521         char cmd[32];
2522         snprintf(cmd, sizeof(cmd), "AT+CMGR=%d\r", index);
2523         return rfcomm_write(hfp->rsock, cmd);
2524 }
2525
2526 /*!
2527  * \brief Start sending an SMS message.
2528  * \param hfp an hfp_pvt struct
2529  * \param number the destination of the message
2530  */
2531 static int hfp_send_cmgs(struct hfp_pvt *hfp, const char *number)
2532 {
2533         char cmd[64];
2534         snprintf(cmd, sizeof(cmd), "AT+CMGS=\"%s\"\r", number);
2535         return rfcomm_write(hfp->rsock, cmd);
2536 }
2537
2538 /*!
2539  * \brief Send the text of an SMS message.
2540  * \param hfp an hfp_pvt struct
2541  * \param message the text of the message
2542  */
2543 static int hfp_send_sms_text(struct hfp_pvt *hfp, const char *message)
2544 {
2545         char cmd[162];
2546         snprintf(cmd, sizeof(cmd), "%.160s\x1a", message);
2547         return rfcomm_write(hfp->rsock, cmd);
2548 }
2549
2550 /*!
2551  * \brief Send AT+CHUP.
2552  * \param hfp an hfp_pvt struct
2553  */
2554 static int hfp_send_chup(struct hfp_pvt *hfp)
2555 {
2556         return rfcomm_write(hfp->rsock, "AT+CHUP\r");
2557 }
2558
2559 /*!
2560  * \brief Send ATD.
2561  * \param hfp an hfp_pvt struct
2562  * \param number the number to send
2563  */
2564 static int hfp_send_atd(struct hfp_pvt *hfp, const char *number)
2565 {
2566         char cmd[64];
2567         snprintf(cmd, sizeof(cmd), "ATD%s;\r", number);
2568         return rfcomm_write(hfp->rsock, cmd);
2569 }
2570
2571 /*!
2572  * \brief Send ATA.
2573  * \param hfp an hfp_pvt struct
2574  */
2575 static int hfp_send_ata(struct hfp_pvt *hfp)
2576 {
2577         return rfcomm_write(hfp->rsock, "ATA\r");
2578 }
2579
2580 /*!
2581  * \brief Send CUSD.
2582  * \param hfp an hfp_pvt struct
2583  * \param code the CUSD code to send
2584  */
2585 static int hfp_send_cusd(struct hfp_pvt *hfp, const char *code)
2586 {
2587         char cmd[128];
2588         snprintf(cmd, sizeof(cmd), "AT+CUSD=1,\"%s\",15\r", code);
2589         return rfcomm_write(hfp->rsock, cmd);
2590 }
2591
2592 /*!
2593  * \brief Parse BRSF data.
2594  * \param hfp an hfp_pvt struct
2595  * \param buf the buffer to parse (null terminated)
2596  */
2597 static int hfp_parse_brsf(struct hfp_pvt *hfp, const char *buf)
2598 {
2599         int brsf;
2600
2601         if (!sscanf(buf, "+BRSF:%d", &brsf))
2602                 return -1;
2603
2604         hfp_int2brsf(brsf, &hfp->brsf);
2605
2606         return 0;
2607 }
2608
2609 /*!
2610  * \brief Parse and store the given indicator.
2611  * \param hfp an hfp_pvt struct
2612  * \param group the indicator group
2613  * \param indicator the indicator to parse
2614  */
2615 static int hfp_parse_cind_indicator(struct hfp_pvt *hfp, int group, char *indicator)
2616 {
2617         int value;
2618
2619         /* store the current indicator */
2620         if (group >= sizeof(hfp->cind_state)) {
2621                 ast_debug(1, "ignoring CIND state '%s' for group %d, we only support up to %d indicators\n", indicator, group, (int) sizeof(hfp->cind_state));
2622                 return -1;
2623         }
2624
2625         if (!sscanf(indicator, "%d", &value)) {
2626                 ast_debug(1, "error parsing CIND state '%s' for group %d\n", indicator, group);
2627                 return -1;
2628         }
2629
2630         hfp->cind_state[group] = value;
2631         return 0;
2632 }
2633
2634 /*!
2635  * \brief Read the result of the AT+CIND? command.
2636  * \param hfp an hfp_pvt struct
2637  * \param buf the buffer to parse (null terminated)
2638  * \note hfp_send_cind_test() and hfp_parse_cind_test() should be called at
2639  * least once before this function is called.
2640  */
2641 static int hfp_parse_cind(struct hfp_pvt *hfp, char *buf)
2642 {
2643         int i, state, group;
2644         size_t s;
2645         char *indicator = NULL;
2646
2647         /* parse current state of all of our indicators.  The list is in the
2648          * following format:
2649          * +CIND: 1,0,2,0,0,0,0
2650          */
2651         group = 0;
2652         state = 0;
2653         s = strlen(buf);
2654         for (i = 0; i < s; i++) {
2655                 switch (state) {
2656                 case 0: /* search for start of the status indicators (a space) */
2657                         if (buf[i] == ' ') {
2658                                 group++;
2659                                 state++;
2660                         }
2661                         break;
2662                 case 1: /* mark this indicator */
2663                         indicator = &buf[i];
2664                         state++;
2665                         break;
2666                 case 2: /* search for the start of the next indicator (a comma) */
2667                         if (buf[i] == ',') {
2668                                 buf[i] = '\0';
2669
2670                                 hfp_parse_cind_indicator(hfp, group, indicator);
2671
2672                                 group++;
2673                                 state = 1;
2674                         }
2675                         break;
2676                 }
2677         }
2678
2679         /* store the last indicator */
2680         if (state == 2)
2681                 hfp_parse_cind_indicator(hfp, group, indicator);
2682
2683         return 0;
2684 }
2685
2686 /*!
2687  * \brief Parse the result of the AT+CIND=? command.
2688  * \param hfp an hfp_pvt struct
2689  * \param buf the buffer to parse (null terminated)
2690  */
2691 static int hfp_parse_cind_test(struct hfp_pvt *hfp, char *buf)
2692 {
2693         int i, state, group;
2694         size_t s;
2695         char *indicator = NULL, *values;
2696
2697         hfp->nocallsetup = 1;
2698
2699         /* parse the indications list.  It is in the follwing format:
2700          * +CIND: ("ind1",(0-1)),("ind2",(0-5))
2701          */
2702         group = 0;
2703         state = 0;
2704         s = strlen(buf);
2705         for (i = 0; i < s; i++) {
2706                 switch (state) {
2707                 case 0: /* search for start of indicator block */
2708                         if (buf[i] == '(') {
2709                                 group++;
2710                                 state++;
2711                         }
2712                         break;
2713                 case 1: /* search for '"' in indicator block */
2714                         if (buf[i] == '"') {
2715                                 state++;
2716                         }
2717                         break;
2718                 case 2: /* mark the start of the indicator name */
2719                         indicator = &buf[i];
2720                         state++;
2721                         break;
2722                 case 3: /* look for the end of the indicator name */
2723                         if (buf[i] == '"') {
2724                                 buf[i] = '\0';
2725                                 state++;
2726                         }
2727                         break;
2728                 case 4: /* find the start of the value range */
2729                         if (buf[i] == '(') {
2730                                 state++;
2731                         }
2732                         break;
2733                 case 5: /* mark the start of the value range */
2734                         values = &buf[i];
2735                         state++;
2736                         break;
2737                 case 6: /* find the end of the value range */
2738                         if (buf[i] == ')') {
2739                                 buf[i] = '\0';
2740                                 state++;
2741                         }
2742                         break;
2743                 case 7: /* process the values we found */
2744                         if (group < sizeof(hfp->cind_index)) {
2745                                 if (!strcmp(indicator, "service")) {
2746                                         hfp->cind_map.service = group;
2747                                         hfp->cind_index[group] = HFP_CIND_SERVICE;
2748                                 } else if (!strcmp(indicator, "call")) {
2749                                         hfp->cind_map.call = group;
2750                                         hfp->cind_index[group] = HFP_CIND_CALL;
2751                                 } else if (!strcmp(indicator, "callsetup")) {
2752                                         hfp->nocallsetup = 0;
2753                                         hfp->cind_map.callsetup = group;
2754                                         hfp->cind_index[group] = HFP_CIND_CALLSETUP;
2755                                 } else if (!strcmp(indicator, "call_setup")) { /* non standard call setup identifier */
2756                                         hfp->nocallsetup = 0;
2757                                         hfp->cind_map.callsetup = group;
2758                                         hfp->cind_index[group] = HFP_CIND_CALLSETUP;
2759                                 } else if (!strcmp(indicator, "callheld")) {
2760                                         hfp->cind_map.callheld = group;
2761                                         hfp->cind_index[group] = HFP_CIND_CALLHELD;
2762                                 } else if (!strcmp(indicator, "signal")) {
2763                                         hfp->cind_map.signal = group;
2764                                         hfp->cind_index[group] = HFP_CIND_SIGNAL;
2765                                 } else if (!strcmp(indicator, "roam")) {
2766                                         hfp->cind_map.roam = group;
2767                                         hfp->cind_index[group] = HFP_CIND_ROAM;
2768                                 } else if (!strcmp(indicator, "battchg")) {
2769                                         hfp->cind_map.battchg = group;
2770                                         hfp->cind_index[group] = HFP_CIND_BATTCHG;
2771                                 } else {
2772                                         hfp->cind_index[group] = HFP_CIND_UNKNOWN;
2773                                         ast_debug(2, "ignoring unknown CIND indicator '%s'\n", indicator);
2774                                 }
2775                         } else {
2776                                         ast_debug(1, "can't store indicator %d (%s), we only support up to %d indicators", group, indicator, (int) sizeof(hfp->cind_index));
2777                         }
2778
2779                         state = 0;
2780                         break;
2781                 }
2782         }
2783
2784         hfp->owner->no_callsetup = hfp->nocallsetup;
2785
2786         return 0;
2787 }
2788
2789
2790 /*
2791  * Bluetooth Headset Profile helpers
2792  */
2793
2794 /*!
2795  * \brief Send an OK AT response.
2796  * \param rsock the rfcomm socket to use
2797  */
2798 static int hsp_send_ok(int rsock)
2799 {
2800         return rfcomm_write(rsock, "\r\nOK\r\n");
2801 }
2802
2803 /*!
2804  * \brief Send an ERROR AT response.
2805  * \param rsock the rfcomm socket to use
2806  */
2807 static int hsp_send_error(int rsock)
2808 {
2809         return rfcomm_write(rsock, "\r\nERROR\r\n");
2810 }
2811
2812 /*!
2813  * \brief Send a speaker gain unsolicited AT response
2814  * \param rsock the rfcomm socket to use
2815  * \param gain the speaker gain value
2816  */
2817 static int hsp_send_vgs(int rsock, int gain)
2818 {
2819         char cmd[32];
2820         snprintf(cmd, sizeof(cmd), "\r\n+VGS=%d\r\n", gain);
2821         return rfcomm_write(rsock, cmd);
2822 }
2823
2824 /*!
2825  * \brief Send a microphone gain unsolicited AT response
2826  * \param rsock the rfcomm socket to use
2827  * \param gain the microphone gain value
2828  */
2829 static int hsp_send_vgm(int rsock, int gain)
2830 {
2831         char cmd[32];
2832         snprintf(cmd, sizeof(cmd), "\r\n+VGM=%d\r\n", gain);
2833         return rfcomm_write(rsock, cmd);
2834 }
2835
2836 /*!
2837  * \brief Send a RING unsolicited AT response.
2838  * \param rsock the rfcomm socket to use
2839  */
2840 static int hsp_send_ring(int rsock)
2841 {
2842         return rfcomm_write(rsock, "\r\nRING\r\n");
2843 }
2844
2845 /*
2846  * message queue functions
2847  */
2848
2849 /*!
2850  * \brief Add an item to the back of the queue.
2851  * \param pvt a mbl_pvt structure
2852  * \param expect the msg we expect to receive
2853  * \param response_to the message that was sent to generate the expected
2854  * response
2855  */
2856 static int msg_queue_push(struct mbl_pvt *pvt, at_message_t expect, at_message_t response_to)
2857 {
2858         struct msg_queue_entry *msg;
2859         if (!(msg = ast_calloc(1, sizeof(*msg)))) {
2860                 return -1;
2861         }
2862         msg->expected = expect;
2863         msg->response_to = response_to;
2864
2865         AST_LIST_INSERT_TAIL(&pvt->msg_queue, msg, entry);
2866         return 0;
2867 }
2868
2869 /*!
2870  * \brief Add an item to the back of the queue with data.
2871  * \param pvt a mbl_pvt structure
2872  * \param expect the msg we expect to receive
2873  * \param response_to the message that was sent to generate the expected
2874  * response
2875  * \param data data associated with this message, it will be freed when the
2876  * message is freed
2877  */
2878 static int msg_queue_push_data(struct mbl_pvt *pvt, at_message_t expect, at_message_t response_to, void *data)
2879 {
2880         struct msg_queue_entry *msg;
2881         if (!(msg = ast_calloc(1, sizeof(*msg)))) {
2882                 return -1;
2883         }
2884         msg->expected = expect;
2885         msg->response_to = response_to;
2886         msg->data = data;
2887
2888         AST_LIST_INSERT_TAIL(&pvt->msg_queue, msg, entry);
2889         return 0;
2890 }
2891
2892 /*!
2893  * \brief Remove an item from the front of the queue.
2894  * \param pvt a mbl_pvt structure
2895  * \return a pointer to the removed item
2896  */
2897 static struct msg_queue_entry *msg_queue_pop(struct mbl_pvt *pvt)
2898 {
2899         return AST_LIST_REMOVE_HEAD(&pvt->msg_queue, entry);
2900 }
2901
2902 /*!
2903  * \brief Remove an item from the front of the queue, and free it.
2904  * \param pvt a mbl_pvt structure
2905  */
2906 static void msg_queue_free_and_pop(struct mbl_pvt *pvt)
2907 {
2908         struct msg_queue_entry *msg;
2909         if ((msg = msg_queue_pop(pvt))) {
2910                 if (msg->data)
2911                         ast_free(msg->data);
2912                 ast_free(msg);
2913         }
2914 }
2915
2916 /*!
2917  * \brief Remove all itmes from the queue and free them.
2918  * \param pvt a mbl_pvt structure
2919  */
2920 static void msg_queue_flush(struct mbl_pvt *pvt)
2921 {
2922         struct msg_queue_entry *msg;
2923         while ((msg = msg_queue_head(pvt)))
2924                 msg_queue_free_and_pop(pvt);
2925 }
2926
2927 /*!
2928  * \brief Get the head of a queue.
2929  * \param pvt a mbl_pvt structure
2930  * \return a pointer to the head of the given msg queue
2931  */
2932 static struct msg_queue_entry *msg_queue_head(struct mbl_pvt *pvt)
2933 {
2934         return AST_LIST_FIRST(&pvt->msg_queue);
2935 }
2936
2937
2938
2939 /*
2940
2941         sdp helpers
2942
2943 */
2944
2945 static int sdp_search(char *addr, int profile)
2946 {
2947
2948         sdp_session_t *session = 0;
2949         bdaddr_t bdaddr;
2950         uuid_t svc_uuid;
2951         uint32_t range = 0x0000ffff;
2952         sdp_list_t *response_list, *search_list, *attrid_list;
2953         int status, port;
2954         sdp_list_t *proto_list;
2955         sdp_record_t *sdprec;
2956
2957         str2ba(addr, &bdaddr);
2958         port = 0;
2959         session = sdp_connect(BDADDR_ANY, &bdaddr, SDP_RETRY_IF_BUSY);
2960         if (!session) {
2961                 ast_debug(1, "sdp_connect() failed on device %s.\n", addr);
2962                 return 0;
2963         }
2964
2965         sdp_uuid32_create(&svc_uuid, profile);
2966         search_list = sdp_list_append(0, &svc_uuid);
2967         attrid_list = sdp_list_append(0, &range);
2968         response_list = 0x00;
2969         status = sdp_service_search_attr_req(session, search_list, SDP_ATTR_REQ_RANGE, attrid_list, &response_list);
2970         if (status == 0) {
2971                 if (response_list) {
2972                         sdprec = (sdp_record_t *) response_list->data;
2973                         proto_list = 0x00;
2974                         if (sdp_get_access_protos(sdprec, &proto_list) == 0) {
2975                                 port = sdp_get_proto_port(proto_list, RFCOMM_UUID);
2976                                 sdp_list_free(proto_list, 0);
2977                         }
2978                         sdp_record_free(sdprec);
2979                         sdp_list_free(response_list, 0);
2980                 } else
2981                         ast_debug(1, "No responses returned for device %s.\n", addr);
2982         } else
2983                 ast_debug(1, "sdp_service_search_attr_req() failed on device %s.\n", addr);
2984
2985         sdp_list_free(search_list, 0);
2986         sdp_list_free(attrid_list, 0);
2987         sdp_close(session);
2988
2989         return port;
2990
2991 }
2992
2993 static sdp_session_t *sdp_register(void)
2994 {
2995
2996         uint32_t service_uuid_int[] = {0, 0, 0, GENERIC_AUDIO_SVCLASS_ID};
2997         uint8_t rfcomm_channel = 1;
2998         const char *service_name = "Asterisk PABX";
2999         const char *service_dsc = "Asterisk PABX";
3000         const char *service_prov = "Asterisk";
3001
3002         uuid_t root_uuid, l2cap_uuid, rfcomm_uuid, svc_uuid, svc_class1_uuid, svc_class2_uuid;
3003         sdp_list_t  *l2cap_list = 0, *rfcomm_list = 0, *root_list = 0, *proto_list = 0, *access_proto_list = 0, *svc_uuid_list = 0;
3004         sdp_data_t *channel = 0;
3005
3006         int err = 0;
3007         sdp_session_t *session = 0;
3008
3009         sdp_record_t *record = sdp_record_alloc();
3010
3011         sdp_uuid128_create(&svc_uuid, &service_uuid_int);
3012         sdp_set_service_id(record, svc_uuid);
3013
3014         sdp_uuid32_create(&svc_class1_uuid, GENERIC_AUDIO_SVCLASS_ID);
3015         sdp_uuid32_create(&svc_class2_uuid, HEADSET_PROFILE_ID);
3016
3017         svc_uuid_list = sdp_list_append(0, &svc_class1_uuid);
3018         svc_uuid_list = sdp_list_append(svc_uuid_list, &svc_class2_uuid);
3019         sdp_set_service_classes(record, svc_uuid_list);
3020
3021         sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
3022         root_list = sdp_list_append(0, &root_uuid);
3023         sdp_set_browse_groups( record, root_list );
3024
3025         sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID);
3026         l2cap_list = sdp_list_append(0, &l2cap_uuid);
3027         proto_list = sdp_list_append(0, l2cap_list);
3028
3029         sdp_uuid16_create(&rfcomm_uuid, RFCOMM_UUID);
3030         channel = sdp_data_alloc(SDP_UINT8, &rfcomm_channel);
3031         rfcomm_list = sdp_list_append(0, &rfcomm_uuid);
3032         sdp_list_append(rfcomm_list, channel);
3033         sdp_list_append(proto_list, rfcomm_list);
3034
3035         access_proto_list = sdp_list_append(0, proto_list);
3036         sdp_set_access_protos(record, access_proto_list);
3037
3038         sdp_set_info_attr(record, service_name, service_prov, service_dsc);
3039
3040         if (!(session = sdp_connect(BDADDR_ANY, BDADDR_LOCAL, SDP_RETRY_IF_BUSY)))
3041                 ast_log(LOG_WARNING, "Failed to connect sdp and create session.\n");
3042         else
3043                 err = sdp_record_register(session, record, 0);
3044
3045         sdp_data_free(channel);
3046         sdp_list_free(rfcomm_list, 0);
3047         sdp_list_free(root_list, 0);
3048         sdp_list_free(access_proto_list, 0);
3049         sdp_list_free(svc_uuid_list, 0);
3050
3051         return session;
3052
3053 }
3054
3055 /*
3056
3057         Thread routines
3058
3059 */
3060
3061 /*!
3062  * \brief Handle the BRSF response.
3063  * \param pvt a mbl_pvt structure
3064  * \param buf a null terminated buffer containing an AT message
3065  * \retval 0 success
3066  * \retval -1 error
3067  */
3068 static int handle_response_brsf(struct mbl_pvt *pvt, char *buf)
3069 {
3070         struct msg_queue_entry *entry;
3071         if ((entry = msg_queue_head(pvt)) && entry->expected == AT_BRSF) {
3072                 if (hfp_parse_brsf(pvt->hfp, buf)) {
3073                         ast_debug(1, "[%s] error parsing BRSF\n", pvt->id);
3074                         goto e_return;
3075                 }
3076
3077                 if (msg_queue_push(pvt, AT_OK, AT_BRSF)) {
3078                         ast_debug(1, "[%s] error handling BRSF\n", pvt->id);
3079                         goto e_return;
3080                 }
3081
3082                 msg_queue_free_and_pop(pvt);
3083         } else if (entry) {
3084                 ast_debug(1, "[%s] received unexpected AT message 'BRSF' when expecting %s, ignoring\n", pvt->id, at_msg2str(entry->expected));
3085         } else {
3086                 ast_debug(1, "[%s] received unexpected AT message 'BRSF'\n", pvt->id);
3087         }
3088
3089         return 0;
3090
3091 e_return:
3092         msg_queue_free_and_pop(pvt);
3093         return -1;
3094 }
3095
3096 /*!
3097  * \brief Handle the CIND response.
3098  * \param pvt a mbl_pvt structure
3099  * \param buf a null terminated buffer containing an AT message
3100  * \retval 0 success
3101  * \retval -1 error
3102  */
3103 static int handle_response_cind(struct mbl_pvt *pvt, char *buf)
3104 {
3105         struct msg_queue_entry *entry;
3106         if ((entry = msg_queue_head(pvt)) && entry->expected == AT_CIND) {
3107                 switch (entry->response_to) {
3108                 case AT_CIND_TEST:
3109                         if (hfp_parse_cind_test(pvt->hfp, buf) || msg_queue_push(pvt, AT_OK, AT_CIND_TEST)) {
3110                                 ast_debug(1, "[%s] error performing CIND test\n", pvt->id);
3111                                 goto e_return;
3112                         }
3113                         break;
3114                 case AT_CIND:
3115                         if (hfp_parse_cind(pvt->hfp, buf) || msg_queue_push(pvt, AT_OK, AT_CIND)) {
3116                                 ast_debug(1, "[%s] error getting CIND state\n", pvt->id);
3117                                 goto e_return;
3118                         }
3119                         break;
3120                 default:
3121                         ast_debug(1, "[%s] error getting CIND state\n", pvt->id);
3122                         goto e_return;
3123                 }
3124                 msg_queue_free_and_pop(pvt);
3125         } else if (entry) {
3126                 ast_debug(1, "[%s] received unexpected AT message 'CIND' when expecting %s, ignoring\n", pvt->id, at_msg2str(entry->expected));
3127         } else {
3128                 ast_debug(1, "[%s] received unexpected AT message 'CIND'\n", pvt->id);
3129         }
3130
3131         return 0;
3132
3133 e_return:
3134         msg_queue_free_and_pop(pvt);
3135         return -1;
3136 }
3137
3138 /*!
3139  * \brief Handle OK AT messages.
3140  * \param pvt a mbl_pvt structure
3141  * \param buf a null terminated buffer containing an AT message
3142  * \retval 0 success
3143  * \retval -1 error
3144  */
3145 static int handle_response_ok(struct mbl_pvt *pvt, char *buf)
3146 {
3147         struct msg_queue_entry *entry;
3148         if ((entry = msg_queue_head(pvt)) && entry->expected == AT_OK) {
3149                 switch (entry->response_to) {
3150
3151                 /* initialization stuff */
3152                 case AT_BRSF:
3153                         ast_debug(1, "[%s] BSRF sent successfully\n", pvt->id);
3154
3155                         /* If this is a blackberry do CMER now, otherwise
3156                          * continue with CIND as normal. */
3157                         if (pvt->blackberry) {
3158                                 if (hfp_send_cmer(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CMER)) {
3159                                         ast_debug(1, "[%s] error sending CMER\n", pvt->id);
3160                                         goto e_return;
3161                                 }
3162                         } else {
3163                                 if (hfp_send_cind_test(pvt->hfp) || msg_queue_push(pvt, AT_CIND, AT_CIND_TEST)) {
3164                                         ast_debug(1, "[%s] error sending CIND test\n", pvt->id);
3165                                         goto e_return;
3166                                 }
3167                         }
3168                         break;
3169                 case AT_CIND_TEST:
3170                         ast_debug(1, "[%s] CIND test sent successfully\n", pvt->id);
3171
3172                         ast_debug(2, "[%s] call: %d\n", pvt->id, pvt->hfp->cind_map.call);
3173                         ast_debug(2, "[%s] callsetup: %d\n", pvt->id, pvt->hfp->cind_map.callsetup);
3174                         ast_debug(2, "[%s] service: %d\n", pvt->id, pvt->hfp->cind_map.service);
3175
3176                         if (hfp_send_cind(pvt->hfp) || msg_queue_push(pvt, AT_CIND, AT_CIND)) {
3177                                 ast_debug(1, "[%s] error requesting CIND state\n", pvt->id);
3178                                 goto e_return;
3179                         }
3180                         break;
3181                 case AT_CIND:
3182                         ast_debug(1, "[%s] CIND sent successfully\n", pvt->id);
3183
3184                         /* check if a call is active */
3185                         if (pvt->hfp->cind_state[pvt->hfp->cind_map.call]) {
3186                                 ast_verb(3, "Bluetooth Device %s has a call in progress - delaying connection.\n", pvt->id);
3187                                 goto e_return;
3188                         }
3189
3190                         /* If this is NOT a blackberry proceed with CMER,
3191                          * otherwise send CLIP. */
3192                         if (!pvt->blackberry) {
3193                                 if (hfp_send_cmer(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CMER)) {
3194                                         ast_debug(1, "[%s] error sending CMER\n", pvt->id);
3195                                         goto e_return;
3196                                 }
3197                         } else {
3198                                 if (hfp_send_clip(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CLIP)) {
3199                                         ast_debug(1, "[%s] error enabling calling line notification\n", pvt->id);
3200                                         goto e_return;
3201                                 }
3202                         }
3203                         break;
3204                 case AT_CMER:
3205                         ast_debug(1, "[%s] CMER sent successfully\n", pvt->id);
3206
3207                         /* If this is a blackberry proceed with the CIND test,
3208                          * otherwise send CLIP. */
3209                         if (pvt->blackberry) {
3210                                 if (hfp_send_cind_test(pvt->hfp) || msg_queue_push(pvt, AT_CIND, AT_CIND_TEST)) {
3211                                         ast_debug(1, "[%s] error sending CIND test\n", pvt->id);
3212                                         goto e_return;
3213                                 }
3214                         } else {
3215                                 if (hfp_send_clip(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CLIP)) {
3216                                         ast_debug(1, "[%s] error enabling calling line notification\n", pvt->id);
3217                                         goto e_return;
3218                                 }
3219                         }
3220                         break;
3221                 case AT_CLIP:
3222                         ast_debug(1, "[%s] caling line indication enabled\n", pvt->id);
3223                         if (hfp_send_vgs(pvt->hfp, 15) || msg_queue_push(pvt, AT_OK, AT_VGS)) {
3224                                 ast_debug(1, "[%s] error synchronizing gain settings\n", pvt->id);
3225                                 goto e_return;
3226                         }
3227
3228                         pvt->timeout = -1;
3229                         pvt->hfp->initialized = 1;
3230                         ast_verb(3, "Bluetooth Device %s initialized and ready.\n", pvt->id);
3231
3232                         break;
3233                 case AT_VGS:
3234                         ast_debug(1, "[%s] volume level synchronization successful\n", pvt->id);
3235
3236                         /* set the SMS operating mode to text mode */
3237                         if (pvt->has_sms) {
3238                                 if (hfp_send_cmgf(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CMGF)) {
3239                                         ast_debug(1, "[%s] error setting CMGF\n", pvt->id);
3240                                         goto e_return;
3241                                 }
3242                         }
3243                         break;
3244                 case AT_CMGF:
3245                         ast_debug(1, "[%s] sms text mode enabled\n", pvt->id);
3246                         /* turn on SMS new message indication */
3247                         if (hfp_send_cnmi(pvt->hfp) || msg_queue_push(pvt, AT_OK, AT_CNMI)) {
3248                                 ast_debug(1, "[%s] error setting CNMI\n", pvt->id);
3249                                 goto e_return;
3250                         }
3251                         break;
3252                 case AT_CNMI:
3253                         ast_debug(1, "[%s] sms new message indication enabled\n", pvt->id);
3254                         pvt->has_sms = 1;
3255                         break;
3256                 /* end initialization stuff */
3257
3258                 case AT_A:
3259                         ast_debug(1, "[%s] answer sent successfully\n", pvt->id);
3260                         pvt->needchup = 1;
3261                         break;
3262                 case AT_D:
3263                         ast_debug(1, "[%s] dial sent successfully\n", pvt->id);
3264                         pvt->needchup = 1;
3265                         pvt->outgoing = 1;
3266                         mbl_queue_control(pvt, AST_CONTROL_PROGRESS);
3267                         break;
3268                 case AT_CHUP:
3269                         ast_debug(1, "[%s] successful hangup\n", pvt->id);
3270                         break;
3271                 case AT_CMGS:
3272                         ast_debug(1, "[%s] successfully sent sms message\n", pvt->id);
3273                         pvt->outgoing_sms = 0;
3274                         break;
3275                 case AT_VTS:
3276                         ast_debug(1, "[%s] digit sent successfully\n", pvt->id);
3277                         break;
3278                 case AT_CUSD:
3279                         ast_debug(1, "[%s] CUSD code sent successfully\n", pvt->id);
3280                         break;
3281                 case AT_UNKNOWN:
3282                 default:
3283                         ast_debug(1, "[%s] received OK for unhandled request: %s\n", pvt->id, at_msg2str(entry->response_to));
3284                         break;
3285                 }
3286                 msg_queue_free_and_pop(pvt);
3287         } else if (entry) {
3288                 ast_debug(1, "[%s] received AT message 'OK' when expecting %s, ignoring\n", pvt->id, at_msg2str(entry->expected));
3289         } else {
3290                 ast_debug(1, "[%s] received unexpected AT message 'OK'\n", pvt->id);
3291         }
3292         return 0;
3293
3294 e_return:
3295         msg_queue_free_and_pop(pvt);
3296         return -1;
3297 }
3298
3299 /*!
3300  * \brief Handle ERROR AT messages.
3301  * \param pvt a mbl_pvt structure
3302  * \param buf a null terminated buffer containing an AT message
3303  * \retval 0 success
3304  * \retval -1 error
3305  */
3306 static int handle_response_error(struct mbl_pvt *pvt, char *buf)
3307 {
3308         struct msg_queue_entry *entry;
3309         if ((entry = msg_queue_head(pvt))
3310                         && (entry->expected == AT_OK
3311                         || entry->expected == AT_ERROR
3312                         || entry->expected == AT_CMS_ERROR
3313                         || entry->expected == AT_CMGR
3314                         || entry->expected == AT_SMS_PROMPT)) {
3315                 switch (entry->response_to) {
3316
3317                 /* initialization stuff */
3318                 case AT_BRSF:
3319                         ast_debug(1, "[%s] error reading BSRF\n", pvt->id);
3320                         goto e_return;
3321                 case AT_CIND_TEST:
3322                         ast_debug(1, "[%s] error during CIND test\n", pvt->id);
3323                         goto e_return;
3324                 case AT_CIND:
3325                         ast_debug(1, "[%s] error requesting CIND state\n", pvt->id);
3326                         goto e_return;
3327                 case AT_CMER:
3328                         ast_debug(1, "[%s] error during CMER request\n", pvt->id);
3329                         goto e_return;
3330                 case AT_CLIP:
3331                         ast_debug(1, "[%s] error enabling calling line indication\n", pvt->id);
3332                         goto e_return;
3333                 case AT_VGS:
3334                         ast_debug(1, "[%s] volume level synchronization failed\n", pvt->id);
3335
3336                         /* this is not a fatal error, let's continue with initialization */
3337
3338                         /* set the SMS operating mode to text mode */
3339                         if (hfp_send_cmgf(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CMGF)) {
3340                                 ast_debug(1, "[%s] error setting CMGF\n", pvt->id);
3341                                 goto e_return;
3342                         }
3343                         break;