AT: dequeue sync requests
[hayes-ril.git] / at.c
1 /*
2  * This file is part of hayes-ril.
3  *
4  * Copyright (C) 2012 Paul Kocialkowski <contact@paulk.fr>
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  *
10  *     http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  */
18
19 #include <string.h>
20 #include <ctype.h>
21
22 #define LOG_TAG "RIL-AT"
23 #include <utils/Log.h>
24
25 #include <hayes-ril.h>
26
27 /*
28  * Globals
29  */
30
31 struct at_handling at_handling;
32
33 /*
34  * Utilities
35  */
36
37 int at_strings_compare(char *major, char *minor)
38 {
39         int major_length = strlen(major);
40         int minor_length = strlen(minor);
41
42         // We can't check against the whole major string
43         if(major_length > minor_length)
44                 return 0;
45
46         if(strncmp(major, minor, major_length) == 0)
47                 return 1;
48         else
49                 return 0;
50 }
51
52 int at_strings_raw_compare(char *major, char *minor)
53 {
54         int major_length = strlen(major);
55
56         // Only consider major length (minor might not have \0)
57         if(strncmp(major, minor, major_length) == 0)
58                 return 1;
59         else
60                 return 0;
61 }
62
63 // Major is response string, minor is request (reference) string
64 int at_commands_compare(char *major, char *minor)
65 {
66         int rc;
67
68         // Assume major is corect
69
70         if(at_strings_compare(major, minor))
71                 return 1;
72
73         // Compare without AT prefix
74         if(at_strings_compare("AT", minor) && !at_strings_compare("AT", major) && strlen(minor) > 2)
75                 return at_strings_compare(major, minor + 2);
76
77         if(at_strings_compare("AT", major) && !at_strings_compare("AT", minor) && strlen(major) > 2)
78                 return at_strings_compare(major + 2, minor);
79
80         return 0;
81 }
82
83 void at_string_clean(char *string, int length)
84 {
85         int i=0;
86
87         for(i=length-1; i >= 0 ; i--) {
88                 if(!isprint(string[i]))
89                         string[i] = '\0';
90         }
91 }
92
93 int at_status_error(int status)
94 {
95         switch(status) {
96                 case AT_STATUS_CONNECT:
97                         return 0;
98                 case AT_STATUS_OK:
99                         return 0;
100                 default:
101                         return 1;
102         }
103 }
104
105 /*
106  * Line parsers
107  */
108
109 int at_line_parse_status(char *data, int length, char **error_p)
110 {
111         char *response_command = NULL;
112         char *response_error = NULL;
113         int response_error_length = 0;
114
115         *error_p = NULL;
116
117         if(at_strings_compare("OK", data)) {
118                 *error_p = NULL;
119                 return AT_STATUS_OK;
120         }
121
122         if(at_strings_compare("CONNECT", data)) {
123                 *error_p = NULL;
124                 return AT_STATUS_CONNECT;
125         }
126
127         if(at_strings_compare("ERROR", data)) {
128                 *error_p = NULL;
129                 return AT_STATUS_ERROR;
130         }
131
132         if(at_strings_compare("+CME ERROR", data)) {
133                 response_error_length = at_line_parse_command_data(data, length, &response_command, &response_error);
134                 if(response_error != NULL && response_error_length > 0) {
135                         at_string_clean(response_error, strlen(response_error) + 1);
136                         *error_p = response_error;
137                 }
138
139                 if(response_command != NULL)
140                         free(response_command);
141
142                 return AT_STATUS_CME_ERROR;
143         }
144
145         return AT_STATUS_UNDEF;
146 }
147
148 int at_line_parse_command_data(char *data, int length, char **response_command, char **response_data)
149 {
150         char *string = NULL;
151         int string_length = 0;
152         int close_your_eyes = 0;
153         int mark = 0;
154         int i;
155
156
157         for(i=0 ; i < length ; i++) {
158                 if(data[i] == ':' && mark == 0) {
159                         if(i > 0) {
160                                 data[i] = '\0';
161
162                                 string_length = i + 1;
163                                 string = strndup(data, string_length);
164
165                                 if(!isprint(string[0])) {
166                                         free(string);
167                                 } else {
168                                         *response_command = string;
169                                 }
170
171                                 mark = i + 1;
172                         }
173
174                         while(isspace(data[i+1])) {
175                                 mark = i + 2;
176                                 i++;
177                         }
178                 }
179
180                 if(data[i] == '"') {
181                         if(close_your_eyes & AT_PARSE_DOUBLE_QUOTE)
182                                 close_your_eyes &= ~AT_PARSE_DOUBLE_QUOTE;
183                         else
184                                 close_your_eyes |= AT_PARSE_DOUBLE_QUOTE;
185                 }
186
187                 if(data[i] == '\'') {
188                         if(close_your_eyes & AT_PARSE_SINGLE_QUOTE)
189                                 close_your_eyes &= ~AT_PARSE_SINGLE_QUOTE;
190                         else
191                                 close_your_eyes |= AT_PARSE_SINGLE_QUOTE;
192                 }
193
194                 // Found = or ? outside of any quotes: assume no data
195                 if(!close_your_eyes && (data[i] == '=' || data[i] == '?') && mark == 0) {
196                         data[i] = '\0';
197
198                         string_length = i + 1;
199                         string = strndup(data, string_length);
200
201                         if(!isprint(string[0])) {
202                                 free(string);
203                         } else {
204                                 *response_command = string;
205                         }
206
207                         return 0;
208                 }
209         }
210
211
212         if(length - mark > 0) {
213                 string_length = length - mark;
214                 string = strndup(data + mark, string_length);
215
216                 if(mark == 0) {
217                         *response_command = string;
218                         return 0;
219                 } else {
220                         *response_data = string;
221                         return string_length;
222                 }
223         }
224
225         return 0;
226 }
227
228 /*
229  * Responses line processor
230  */
231
232 int at_responses_process_line(struct at_response ***responses_p, int responses_count, char *data, int length)
233 {
234         struct at_response **responses = NULL;
235
236         char *response_command = NULL;
237         char *response_data = NULL;
238         int response_data_length = 0;
239         char *response_error = NULL;
240         int response_status = 0;
241
242         char **response_previous_data = NULL;
243         int response_data_count = 0;
244
245         int index = -1;
246         int count = 0;
247         int i;
248
249         if(responses_p == NULL || data == NULL || length < 0) {
250                 LOGE("Failed to process AT response: wrong arguments!");
251                 return 0;
252         }
253
254         responses = *responses_p;
255
256         // Parse status
257         response_status = at_line_parse_status(data, length, &response_error);
258
259         if(response_status != AT_STATUS_UNDEF) {
260                 if(responses_count > 0 && responses != NULL) {
261                         for(i=responses_count-1 ; i >= 0; i--) {
262                                 if(responses[i]->status == AT_STATUS_UNDEF) {
263                                         responses[i]->status = response_status;
264                                         responses[i]->error = response_error;
265
266                                         // Do not alloc a new response
267                                         if(index == -1)
268                                                 index = i;
269                                 }
270                         }
271                 }
272
273                 // Alloc a new response
274                 if(index == -1) {
275                         // Index is the request index in the requests array
276                         index = responses_count;
277                         // Count is the total count of requests in the array
278                         count = index + 1;
279
280                         // Alloc the array with the new size
281                         *responses_p = malloc(sizeof(struct at_response *) * count);
282
283                         // Copy and free previous data
284                         if(responses != NULL && responses_count > 0) {
285                                 memcpy(*responses_p, responses, sizeof(struct at_response *) * responses_count);
286                                 free(responses);
287                         }
288
289                         responses = *responses_p;
290
291                         // Alloc new structure and copy obtained data
292                         responses[index] = calloc(1, sizeof(struct at_response));
293                         responses[index]->status = response_status;
294                         responses[index]->error = response_error;
295                 }
296         } else {
297                 // Parse command and data
298                 response_data_length = at_line_parse_command_data(data, length, &response_command, &response_data);
299
300                 if(response_command == NULL) {
301                         LOGE("Failed to parse command!");
302                         return responses_count;
303                 }
304
305                 at_string_clean(response_command, strlen(response_command) + 1);
306
307                 if(responses_count > 0 && responses != NULL) {
308                         for(i=responses_count-1 ; i >= 0; i--) {
309                                 if(responses[i]->command != NULL) {
310                                         if(at_commands_compare(responses[i]->command, response_command))
311                                                 // Do not alloc a new response
312                                                 index = i;
313                                 }
314                         }
315                 }
316
317                 // Alloc a new response
318                 if(index == -1) {
319                         // Index is the request index in the requests array
320                         index = responses_count;
321                         // Count is the total count of requests in the array
322                         count = index + 1;
323
324                         // Alloc the array with the new size
325                         *responses_p = malloc(sizeof(struct at_response *) * count);
326
327                         // Copy and free previous data
328                         if(responses != NULL && responses_count > 0) {
329                                 memcpy(*responses_p, responses, sizeof(struct at_response *) * responses_count);
330                                 free(responses);
331                         }
332
333                         responses = *responses_p;
334
335                         // Alloc new structure and copy obtained data
336                         responses[index] = calloc(1, sizeof(struct at_response));
337                         responses[index]->command = response_command;
338                         responses[index]->status = AT_STATUS_UNDEF;
339                 }
340
341                 if(response_data_length > 0 && response_data != NULL) {
342                         at_string_clean(response_data, response_data_length);
343
344                         response_previous_data = responses[index]->data;
345
346                         // Data count is the total count of elements in the request data
347                         response_data_count = responses[index]->data_count + 1;
348
349                         // Alloc the array with the new size
350                         responses[index]->data = malloc(sizeof(char *) * response_data_count);
351
352                         // Copy and free previous data
353                         if(response_previous_data != NULL) {
354                                 memcpy(responses[index]->data, response_previous_data, sizeof(char *) * responses[index]->data_count);
355                                 free(response_previous_data);
356                         }
357
358                         responses[index]->data_count = response_data_count;
359                         responses[index]->data[response_data_count - 1] = response_data;
360                 }
361         }
362
363         return count > responses_count ? count : responses_count;
364 }
365
366 /*
367  * Responses structures processing
368  */
369
370 int at_responses_process(struct at_response ***responses_p, char *data, int length)
371 {
372         int responses_count = 0;
373         int count = 0;
374
375         char *string = NULL;
376         int string_length = 0;
377         int mark = 0;
378         int i;
379
380         if(responses_p == NULL || data == NULL || length < 0) {
381                 LOGE("Failed to process AT response: wrong arguments!");
382                 return 0;
383         }
384
385         for(i=0 ; i < length ; i++) {
386                 if(data[i] == '\r' || data[i] == ';') {
387                         if(i - mark > 0) {
388                                 data[i] = '\0';
389
390                                 string_length = i - mark + 1;
391                                 string = strndup(data + mark, string_length);
392
393                                 if(!isprint(string[0])) {
394                                         free(string);
395                                 }
396                                 else {
397                                         count = at_responses_process_line(responses_p, responses_count, string, string_length);
398                                         if(count > responses_count)
399                                                 responses_count = count;
400
401                                         free(string);
402                                 }
403
404                                 mark = i + 1;
405                         }
406
407                         while(isspace(data[i+1])) {
408                                 mark = i + 2;
409                                 i++;
410                         }
411                 }
412         }
413
414         if(length - mark > 0) {
415                 for(i=mark ; i < length ; i++)
416                         if(!isprint(data[i]))
417                                 break;
418
419                 if(i - mark > 0) {
420                         string_length = i - mark + 1;
421                         string = calloc(1, string_length);
422
423                         memcpy(string, data + mark, string_length - 1);
424                         string[string_length - 1] = '\0';
425
426                         count = at_responses_process_line(responses_p, responses_count, string , string_length);
427                         if(count > responses_count)
428                                 responses_count = count;
429
430                         free(string);
431                 }
432         }
433
434         return responses_count;
435 }
436
437 void at_response_free(struct at_response *response)
438 {
439         int i;
440
441         if(response->data_count > 0 && response->data != NULL) {
442                 for(i=0 ; i < response->data_count ; i++) {
443                         if(response->data[i] != NULL) {
444                                 free(response->data[i]);
445                                 response->data[i] = NULL;
446                         }
447                 }
448
449                 free(response->data);
450                 response->data_count = 0;
451         }
452
453         if(response->command != NULL)
454                 free(response->command);
455
456         if(response->error != NULL)
457                 free(response->error);
458
459         memset(response, 0, sizeof(struct at_response));
460
461 }
462
463 /*
464  * Response data parsing
465  */
466
467 char *at_response_data_parse_string(char *data, int length)
468 {
469         char *string = NULL;
470         int string_length = 0;
471         int mark = 0;
472         int quote = 0;
473         int i;
474
475         for(i=0 ; i < length ; i++) {
476                 if(data[i] == '"') {
477                         if(quote & AT_PARSE_DOUBLE_QUOTE) {
478                                 if(i - mark > 0) {
479                                         data[i] = '\0';
480
481                                         string_length = i - mark + 1;
482                                         string = strndup(data + mark, string_length);
483
484                                         if(!isprint(string[0])) {
485                                                 free(string);
486                                                 return NULL;
487                                         }
488                                         else {
489                                                 return string;
490                                         }
491                                 } else if(i - mark == 0) {
492                                         return strdup("");
493                                 }
494                         } else {
495                                 quote |= AT_PARSE_DOUBLE_QUOTE;
496                                 mark = i + 1;
497                         }
498                 }
499         }
500
501         // Only return the first string between a pair of "
502
503         return NULL;
504 }
505
506 int at_response_data_parse_numeric(char *data, int length)
507 {
508         int i;
509
510         for(i=0 ; i < length ; i++) {
511                 if(isdigit(data[i])) {
512                         return atoi(data + i);
513                 }
514         }
515
516         return 0;
517 }
518
519 /*
520  * Response data processing
521  */
522
523 int at_response_data_process_value(struct at_response_data ***response_data_p, int response_data_count, char *data, int length)
524 {
525         struct at_response_data **response_data = NULL;
526         char *response_data_string = NULL;
527         int response_data_numeric = 0;
528
529         int index = -1;
530         int count = 0;
531
532         if(response_data_p == NULL || data == NULL || length < 0) {
533                 LOGE("Failed to process AT response data value: wrong arguments!");
534                 return 0;
535         }
536
537         response_data = *response_data_p;
538
539         // Parse string
540         response_data_string = at_response_data_parse_string(data, length);
541         if(response_data_string != NULL) {
542                 // Index is the response data index in the response data array
543                 index = response_data_count;
544                 // Count is the total count of response data in the array
545                 count = index + 1;
546
547                 // Alloc the array with the new size
548                 *response_data_p = malloc(sizeof(struct at_response_data *) * count);
549
550                 // Copy and free previous data
551                 if(response_data != NULL && response_data_count > 0) {
552                         memcpy(*response_data_p, response_data, sizeof(struct at_response_data *) * response_data_count);
553                         free(response_data);
554                 }
555
556                 response_data = *response_data_p;
557
558                 // Alloc new structure and copy obtained data
559                 response_data[index] = calloc(1, sizeof(struct at_response_data));
560                 response_data[index]->type = AT_RESPONSE_DATA_STRING;
561                 response_data[index]->value.s = response_data_string;
562
563         } else {
564                 response_data_numeric = at_response_data_parse_numeric(data, length);
565
566                 // Index is the response data index in the response data array
567                 index = response_data_count;
568                 // Count is the total count of response data in the array
569                 count = index + 1;
570
571                 // Alloc the array with the new size
572                 *response_data_p = malloc(sizeof(struct at_response_data *) * count);
573
574                 // Copy and free previous data
575                 if(response_data != NULL && response_data_count > 0) {
576                         memcpy(*response_data_p, response_data, sizeof(struct at_response_data *) * response_data_count);
577                         free(response_data);
578                 }
579
580                 response_data = *response_data_p;
581
582                 // Alloc new structure and copy obtained data
583                 response_data[index] = calloc(1, sizeof(struct at_response_data));
584                 response_data[index]->type = AT_RESPONSE_DATA_NUMERIC;
585                 response_data[index]->value.n = response_data_numeric;
586         }
587
588         return count > response_data_count ? count : response_data_count;
589 }
590
591 int at_response_data_process(struct at_response_data ***response_data_p, char *data, int length)
592 {
593         int response_data_count = 0;
594         int count = 0;
595
596         char *string = NULL;
597         int string_length = 0;
598         int mark = 0;
599         int i;
600
601         if(response_data_p == NULL || data == NULL || length < 0) {
602                 LOGE("Failed to process AT response data: wrong arguments!");
603                 return 0;
604         }
605
606         for(i=0 ; i < length ; i++) {
607                 if(data[i] == ',') {
608                         if(i - mark > 0) {
609                                 data[i] = '\0';
610
611                                 string_length = i - mark + 1;
612                                 string = strndup(data + mark, string_length);
613
614                                 if(!isprint(string[0])) {
615                                         free(string);
616                                 }
617                                 else {
618                                         count = at_response_data_process_value(response_data_p, response_data_count, string, string_length);
619                                         if(count > response_data_count)
620                                                 response_data_count = count;
621
622                                         free(string);
623                                 }
624
625                                 mark = i + 1;
626                         }
627
628                         while(isspace(data[i+1])) {
629                                 mark = i + 2;
630                                 i++;
631                         }
632                 }
633         }
634
635         if(length - mark > 0) {
636                 for(i=mark ; i < length ; i++)
637                         if(!isprint(data[i]))
638                                 break;
639
640                 if(i - mark > 0) {
641                         string_length = i - mark + 1;
642                         string = calloc(1, string_length);
643
644                         memcpy(string, data + mark, string_length - 1);
645                         string[string_length - 1] = '\0';
646
647                         count = at_response_data_process_value(response_data_p, response_data_count, string, string_length);
648                         if(count > response_data_count)
649                                 response_data_count = count;
650
651                         free(string);
652                 }
653         }
654
655         return response_data_count;
656 }
657
658 void at_response_data_free(struct at_response_data **response_data, int response_data_count)
659 {
660         int i;
661
662         if(response_data == NULL || response_data_count <= 0)
663                 return;
664
665         for(i=0 ; i < response_data_count ; i++) {
666                 if(response_data[i] != NULL) {
667                         if(response_data[i]->type == AT_RESPONSE_DATA_STRING && response_data[i]->value.s != NULL) {
668                                 free(response_data[i]->value.s);
669                         }
670
671                         memset(response_data[i], 0, sizeof(struct at_response_data));
672                 }
673         }
674
675         free(response_data);
676 }
677
678 /*
679  * Handling
680  */
681
682 void at_handling_init(void)
683 {
684         memset(&at_handling, 0, sizeof(struct at_handling));
685
686         pthread_mutex_init(&(at_handling.responses_queue.queue_mutex), NULL);
687         pthread_mutex_init(&(at_handling.responses_queue.mutex), NULL);
688         pthread_mutex_init(&(at_handling.requests_queue.mutex), NULL);
689         pthread_mutex_init(&(at_handling.sync_requests_queue.queue_mutex), NULL);
690         pthread_mutex_init(&(at_handling.sync_requests_queue.mutex), NULL);
691         pthread_mutex_init(&(at_handling.async_requests_queue.mutex), NULL);
692
693         at_handling.freeze = AT_FREEZE_OFF;
694
695         // First lock to the queues mutexes
696         AT_RESPONSES_QUEUE_LOCK();
697         AT_SYNC_QUEUE_LOCK();
698 }
699
700 /*
701  * Responses queue
702  */
703
704 // Unlock the responses queue after queuing all the available requests
705 void at_responses_queue_unlock(void)
706 {
707         if(at_handling.responses_queue.responses_count <= 0 || at_handling.responses_queue.responses == NULL)
708                 return;
709
710         AT_RESPONSES_QUEUE_UNLOCK();
711 }
712
713 // Queue one response to the responses queue
714 int at_response_queue(struct at_response *response)
715 {
716         struct at_response **responses = NULL;
717         int responses_count = 0;
718         int index;
719         int count;
720
721         if(response == NULL)
722                 return -1;
723
724         AT_RESPONSES_LOCK();
725
726         // Save the previous data pointer and count
727         responses = at_handling.responses_queue.responses;
728         responses_count = at_handling.responses_queue.responses_count;
729
730         if(responses_count < 0)
731                 responses_count = 0;
732
733         // Index is the response index in the responses array
734         index = responses_count;
735         // Count is the total count of responses in the array
736         count = index + 1;
737
738         // Alloc the array with the new size
739         at_handling.responses_queue.responses = malloc(sizeof(struct at_response *) * count);
740         at_handling.responses_queue.responses_count = count;
741
742         // Copy and free previous data
743         if(responses != NULL && responses_count > 0) {
744                 memcpy(at_handling.responses_queue.responses, responses, sizeof(struct at_response *) * at_handling.responses_queue.responses_count);
745                 free(responses);
746         }
747
748         // Get the new data pointer and count
749         responses = at_handling.responses_queue.responses;
750         responses_count = at_handling.responses_queue.responses_count;
751
752         // Put the response in the queue
753         responses[index] = response;
754
755         LOGD("%d elements in the responses queue", responses_count);
756
757         AT_RESPONSES_UNLOCK();
758
759         return 0;
760 }
761
762 // Unqueue the oldest response in the queue
763 struct at_response *at_response_dequeue(void)
764 {
765         struct at_response *response = NULL;
766         struct at_response **responses = NULL;
767         int responses_count = 0;
768         int pos = -1;
769         int i;
770
771         AT_RESPONSES_LOCK();
772
773         // Save the previous data pointer and count
774         responses = at_handling.responses_queue.responses;
775         responses_count = at_handling.responses_queue.responses_count;
776
777         while(responses_count <= 0 || responses == NULL) {
778                 LOGE("No response queued, blocking!");
779
780                 AT_RESPONSES_UNLOCK();
781                 AT_RESPONSES_QUEUE_LOCK();
782                 AT_RESPONSES_LOCK();
783
784                 if(at_freeze_get() == AT_FREEZE_SEND) {
785                         AT_RESPONSES_UNLOCK();
786                         return NULL;
787                 }
788
789                 // Get the new data pointer and count
790                 responses = at_handling.responses_queue.responses;
791                 responses_count = at_handling.responses_queue.responses_count;
792
793                 LOGE("Unblocking: %d new responses queued!", responses_count);
794         }
795
796         for(i=0 ; i < responses_count ; i++) {
797                 if(responses[i] != NULL) {
798                         response = responses[i];
799                         pos = i;
800                         break;
801                 }
802         }
803
804         if(response == NULL || pos < 0) {
805                 LOGD("Found no valid response, aborting!");
806
807                 AT_RESPONSES_UNLOCK();
808                 return NULL;
809         }
810
811         // Empty the found position in the responses array
812         responses[pos] = NULL;
813
814         // Move the elements back
815         for(i=pos ; i < responses_count-1 ; i++) {
816                 responses[i] = responses[i+1];
817         }
818
819         // Empty the latest element
820         if(pos != responses_count-1) {
821                 responses[responses_count-1] = NULL;
822         }
823
824         responses_count--;
825
826         if(responses_count == 0) {
827                 free(responses);
828                 at_handling.responses_queue.responses = NULL;
829         }
830
831         at_handling.responses_queue.responses_count = responses_count;
832
833         LOGD("%d elements in the responses queue", responses_count);
834
835         AT_RESPONSES_UNLOCK();
836
837         return response;
838 }
839
840 /*
841  * Requests queue
842  */
843
844 // Queue one request to the requests queue
845 int at_request_queue(struct at_request *request)
846 {
847         struct at_request **requests = NULL;
848         int requests_count = 0;
849         int index;
850         int count;
851
852         if(request == NULL)
853                 return -1;
854
855         AT_REQUESTS_LOCK();
856
857         // Save the previous data pointer and count
858         requests = at_handling.requests_queue.requests;
859         requests_count = at_handling.requests_queue.requests_count;
860
861         if(requests_count < 0)
862                 requests_count = 0;
863
864         // Index is the request index in the requests array
865         index = requests_count;
866         // Count is the total count of requests in the array
867         count = index + 1;
868
869         // Alloc the array with the new size
870         at_handling.requests_queue.requests = malloc(sizeof(struct at_request *) * count);
871         at_handling.requests_queue.requests_count = count;
872
873         // Copy and free previous data
874         if(requests != NULL && requests_count > 0) {
875                 memcpy(at_handling.requests_queue.requests, requests, sizeof(struct at_request *) * at_handling.requests_queue.requests_count);
876                 free(requests);
877         }
878
879         // Get the new data pointer and count
880         requests = at_handling.requests_queue.requests;
881         requests_count = at_handling.requests_queue.requests_count;
882
883         // Put the request in the queue
884         requests[index] = request;
885
886         LOGD("%d elements in the requests queue", requests_count);
887
888         AT_REQUESTS_UNLOCK();
889
890         return 0;
891 }
892
893 // Unqueue the oldest request in the queue
894 struct at_request *at_request_dequeue(void)
895 {
896         struct at_request *request = NULL;
897         struct at_request **requests = NULL;
898         int requests_count = 0;
899         int pos = -1;
900         int i;
901
902         AT_REQUESTS_LOCK();
903
904         // Save the previous data pointer and count
905         requests = at_handling.requests_queue.requests;
906         requests_count = at_handling.requests_queue.requests_count;
907
908         if(requests_count <= 0 || requests == NULL) {
909                 LOGE("No requests queued!");
910
911                 AT_REQUESTS_UNLOCK();
912                 return NULL;
913         }
914
915         for(i=0 ; i < requests_count ; i++) {
916                 if(requests[i] != NULL) {
917                         request = requests[i];
918                         pos = i;
919                         break;
920                 }
921         }
922
923         if(requests == NULL || pos < 0) {
924                 LOGD("Found no valid request, aborting!");
925
926                 AT_REQUESTS_UNLOCK();
927                 return NULL;
928         }
929
930         // Empty the found position in the requests array
931         requests[pos] = NULL;
932
933         // Move the elements back
934         for(i=pos ; i < requests_count-1 ; i++) {
935                 requests[i] = requests[i+1];
936         }
937
938         // Empty the latest element
939         if(pos != requests_count-1) {
940                 requests[requests_count-1] = NULL;
941         }
942
943         requests_count--;
944
945         if(requests_count == 0) {
946                 free(requests);
947                 at_handling.requests_queue.requests = NULL;
948         }
949
950         at_handling.requests_queue.requests_count = requests_count;
951
952         LOGD("%d elements in the requests queue", requests_count);
953
954         AT_REQUESTS_UNLOCK();
955
956         return request;
957 }
958
959 /*
960  * Async requests queue
961  */
962
963 // Free the async request
964 void at_async_request_free(struct at_async_request *async_request)
965 {
966         if(async_request == NULL)
967                 return;
968
969         if(async_request->command)
970                 free(async_request->command);
971
972         if(async_request->request)
973                 at_request_free(async_request->request);
974
975         memset(async_request, 0, sizeof(struct at_async_request));
976         async_request->handled = AT_RESPONSE_GARBAGE;
977 }
978
979 // Find an async request from its command
980 struct at_async_request *at_async_request_find_command(char *command)
981 {
982         struct at_async_request *async_request = NULL;
983         struct at_async_request **async_requests = NULL;
984         int async_requests_count = 0;
985         int i;
986
987         if(command == NULL)
988                 return NULL;
989
990         AT_ASYNC_LOCK();
991
992         // Save the previous data pointer and count
993         async_requests = at_handling.async_requests_queue.async_requests;
994         async_requests_count = at_handling.async_requests_queue.async_requests_count;
995
996         if(async_requests_count <= 0 || async_requests == NULL) {
997                 AT_ASYNC_UNLOCK();
998                 return NULL;
999         }
1000
1001         for(i=0 ; i < async_requests_count ; i++) {
1002                 if(async_requests[i] != NULL && async_requests[i]->command != NULL) {
1003                         if(at_commands_compare(command, async_requests[i]->command)) {
1004                                 async_request = async_requests[i];
1005                                 break;
1006                         }
1007                 }
1008         }
1009
1010         AT_ASYNC_UNLOCK();
1011
1012         return async_request;
1013 }
1014
1015 // Find an async request from its handled status
1016 struct at_async_request *at_async_request_find_handled(int handled)
1017 {
1018         struct at_async_request *async_request = NULL;
1019         struct at_async_request **async_requests = NULL;
1020         int async_requests_count = 0;
1021         int i;
1022
1023         AT_ASYNC_LOCK();
1024
1025         // Save the previous data pointer and count
1026         async_requests = at_handling.async_requests_queue.async_requests;
1027         async_requests_count = at_handling.async_requests_queue.async_requests_count;
1028
1029         if(async_requests_count <= 0 || async_requests == NULL) {
1030                 AT_ASYNC_UNLOCK();
1031                 return NULL;
1032         }
1033
1034         for(i=0 ; i < async_requests_count ; i++) {
1035                 if(async_requests[i] != NULL) {
1036                         if(async_requests[i]->handled == handled) {
1037                                 async_request = async_requests[i];
1038                                 break;
1039                         }
1040                 }
1041         }
1042
1043         AT_ASYNC_UNLOCK();
1044
1045         return async_request;
1046 }
1047
1048 // Find an async request from its request pointer
1049 struct at_async_request *at_async_request_find_request(struct at_request *request)
1050 {
1051         struct at_async_request *async_request = NULL;
1052         struct at_async_request **async_requests = NULL;
1053         int async_requests_count = 0;
1054         int i;
1055
1056         if(request == NULL)
1057                 return NULL;
1058
1059         AT_ASYNC_LOCK();
1060
1061         // Save the previous data pointer and count
1062         async_requests = at_handling.async_requests_queue.async_requests;
1063         async_requests_count = at_handling.async_requests_queue.async_requests_count;
1064
1065         if(async_requests_count <= 0 || async_requests == NULL) {
1066                 AT_ASYNC_UNLOCK();
1067                 return NULL;
1068         }
1069
1070         for(i=0 ; i < async_requests_count ; i++) {
1071                 if(async_requests[i] != NULL) {
1072                         if(async_requests[i]->request == request) {
1073                                 async_request = async_requests[i];
1074                                 break;
1075                         }
1076                 }
1077         }
1078
1079         AT_ASYNC_UNLOCK();
1080
1081         return async_request;
1082 }
1083
1084 // Find an async request from its response pointer
1085 struct at_async_request *at_async_request_find_response(struct at_response *response)
1086 {
1087         struct at_async_request *async_request = NULL;
1088         struct at_async_request **async_requests = NULL;
1089         int async_requests_count = 0;
1090         int i;
1091
1092         if(response == NULL)
1093                 return NULL;
1094
1095         AT_ASYNC_LOCK();
1096
1097         // Save the previous data pointer and count
1098         async_requests = at_handling.async_requests_queue.async_requests;
1099         async_requests_count = at_handling.async_requests_queue.async_requests_count;
1100
1101         if(async_requests_count <= 0 || async_requests == NULL) {
1102                 AT_ASYNC_UNLOCK();
1103                 return NULL;
1104         }
1105
1106         for(i=0 ; i < async_requests_count ; i++) {
1107                 if(async_requests[i] != NULL && async_requests[i]->response != NULL) {
1108                         if(async_requests[i]->response == response) {
1109                                 async_request = async_requests[i];
1110                                 break;
1111                         }
1112                 }
1113         }
1114
1115         AT_ASYNC_UNLOCK();
1116
1117         return async_request;
1118 }
1119
1120 // Queue one request to the async requests queue
1121 int at_async_request_queue(struct at_async_request *async_request)
1122 {
1123         struct at_async_request **async_requests = NULL;
1124         int async_requests_count = 0;
1125         int index;
1126         int count;
1127
1128         if(async_request == NULL)
1129                 return -1;
1130
1131         AT_ASYNC_LOCK();
1132
1133         // Save the previous data pointer and count
1134         async_requests = at_handling.async_requests_queue.async_requests;
1135         async_requests_count = at_handling.async_requests_queue.async_requests_count;
1136
1137         if(async_requests_count < 0)
1138                 async_requests_count = 0;
1139
1140         // Index is the sync request index in the sync requests array
1141         index = async_requests_count;
1142         // Count is the total count of sync requests in the array
1143         count = index + 1;
1144
1145         // Alloc the array with the new size
1146         at_handling.async_requests_queue.async_requests = malloc(sizeof(struct at_async_request *) * count);
1147         at_handling.async_requests_queue.async_requests_count = count;
1148
1149         // Copy and free previous data
1150         if(async_requests != NULL && async_requests_count > 0) {
1151                 memcpy(at_handling.async_requests_queue.async_requests, async_requests, sizeof(struct at_async_request *) * at_handling.async_requests_queue.async_requests_count);
1152                 free(async_requests);
1153         }
1154
1155         // Get the new data pointer and count
1156         async_requests = at_handling.async_requests_queue.async_requests;
1157         async_requests_count = at_handling.async_requests_queue.async_requests_count;
1158
1159         // Put the sync request in the queue
1160         async_requests[index] = async_request;
1161
1162         LOGD("%d elements in the async requests queue", async_requests_count);
1163
1164         AT_ASYNC_UNLOCK();
1165
1166         return 0;
1167 }
1168
1169 int at_async_response_dequeue(struct at_response *response)
1170 {
1171         struct at_async_request *async_request = NULL;
1172         int rc;
1173
1174         if(response == NULL)
1175                 return -1;
1176
1177         // First, try to grab the async request from the response, if it was already filled by sync dequeue
1178         async_request = at_async_request_find_response(response);
1179         if(async_request == NULL || async_request->func == NULL) {
1180                 // Then, try to find an unhandled response
1181
1182                 async_request = at_async_request_find_handled(AT_RESPONSE_UNHANDELD_REASON_STATUS);
1183                 if(async_request == NULL || async_request->func == NULL) {
1184                         // Finally, grab the async request from the command
1185
1186                         async_request = at_async_request_find_command(response->command);
1187                         if(async_request == NULL || async_request->func == NULL) {
1188                                 return -1;
1189                         }
1190                 }
1191
1192                 // FIXME: What if there is already a response with valid data?
1193
1194                 async_request->response = response;
1195         }
1196
1197         LOGD("Found a matching request for %s!", async_request->command);
1198
1199         // This prevents sync data not to be reported when the same command is issued in func and previous handled was unhandled for status reason
1200         async_request->handled = AT_RESPONSE_SENT;
1201
1202         at_async_request_dequeue(async_request);
1203
1204         rc = async_request->func(response, async_request->data, async_request->token);
1205
1206         // If the request was unhandled, update its status and requeue
1207         if(rc == AT_RESPONSE_UNHANDELD_REASON_STATUS) {
1208                 LOGD("Response not handled (missing status)");
1209                 async_request->handled = AT_RESPONSE_UNHANDELD_REASON_STATUS;
1210                 at_async_request_queue(async_request);
1211         } else if(rc == AT_RESPONSE_UNHANDELD_REASON_DATA) {
1212                 LOGD("Response not handled (missing data)");
1213                 async_request->handled = AT_RESPONSE_UNHANDELD_REASON_DATA;
1214                 at_async_request_queue(async_request);
1215         } else {
1216                 LOGD("Response was handled!");
1217
1218                 // We can free the request
1219                 at_async_request_free(async_request);
1220
1221                 // Send the next request in the queue
1222                 at_send_next_request();
1223         }
1224
1225         return 0;
1226 }
1227
1228 // Dequeue one async request (only remove from the queue)
1229 int at_async_request_dequeue(struct at_async_request *async_request)
1230 {
1231         struct at_async_request **async_requests = NULL;
1232         int async_requests_count = 0;
1233         int pos = -1;
1234         int i;
1235
1236         if(async_request == NULL)
1237                 return -1;
1238
1239         AT_ASYNC_LOCK();
1240
1241         // Save the previous data pointer and count
1242         async_requests = at_handling.async_requests_queue.async_requests;
1243         async_requests_count = at_handling.async_requests_queue.async_requests_count;
1244
1245         if(async_requests_count <= 0 || async_requests == NULL) {
1246                 LOGE("No async requests queued, aborting dequeue!");
1247
1248                 AT_ASYNC_UNLOCK();
1249                 return -1;
1250         }
1251
1252         for(i=0 ; i < async_requests_count ; i++) {
1253                 if(async_requests[i] == async_request) {
1254                         pos = i;
1255                         break;
1256                 }
1257         }
1258
1259         if(pos < 0) {
1260                 LOGD("Found no matching async request, aborting dequeue!");
1261
1262                 AT_ASYNC_UNLOCK();
1263                 return -1;
1264         }
1265
1266         // Empty the found position in the async requests array
1267         async_requests[pos] = NULL;
1268
1269         // Move the elements back
1270         for(i=pos ; i < async_requests_count-1 ; i++) {
1271                 async_requests[i] = async_requests[i+1];
1272         }
1273
1274         // Empty the latest element
1275         if(pos != async_requests_count-1) {
1276                 async_requests[async_requests_count-1] = NULL;
1277         }
1278
1279         async_requests_count--;
1280
1281         if(async_requests_count == 0) {
1282                 free(async_requests);
1283                 at_handling.async_requests_queue.async_requests = NULL;
1284         }
1285
1286         at_handling.async_requests_queue.async_requests_count = async_requests_count;
1287
1288         LOGD("%d elements in the async requests queue", async_requests_count);
1289
1290         AT_ASYNC_UNLOCK();
1291
1292         return 0;
1293 }
1294
1295 /*
1296  * Sync requests queue
1297  */
1298
1299 // Lock the sync requests queue
1300 void at_sync_requests_queue_lock(void)
1301 {
1302         AT_SYNC_QUEUE_LOCK();
1303 }
1304
1305 // Unlock the sync requests queue
1306 void at_sync_requests_queue_unlock(void)
1307 {
1308         AT_SYNC_QUEUE_UNLOCK();
1309 }
1310
1311 // Free the sync request
1312 void at_sync_request_free(struct at_sync_request *sync_request)
1313 {
1314         if(sync_request == NULL)
1315                 return;
1316
1317         if(sync_request->command)
1318                 free(sync_request->command);
1319
1320         if(sync_request->request)
1321                 at_request_free(sync_request->request);
1322
1323         memset(sync_request, 0, sizeof(struct at_sync_request));
1324         sync_request->handled = AT_RESPONSE_GARBAGE;
1325 }
1326
1327 // Whether the sync queue is empty
1328 int at_sync_queue_empty(void)
1329 {
1330         struct at_sync_request *sync_request = NULL;
1331         struct at_sync_request **sync_requests = NULL;
1332         int sync_requests_count;
1333
1334         int empty = 1;
1335
1336         AT_SYNC_LOCK();
1337
1338         // Save the previous data pointer and count
1339         sync_requests = at_handling.sync_requests_queue.sync_requests;
1340         sync_requests_count = at_handling.sync_requests_queue.sync_requests_count;
1341
1342         if(sync_requests_count <= 0 || sync_requests == NULL)
1343                 empty = 1;
1344         else
1345                 empty = 0;
1346
1347         AT_SYNC_UNLOCK();
1348
1349         return empty;
1350 }
1351
1352 // Find a sync request from its command
1353 struct at_sync_request *at_sync_request_find_command(char *command)
1354 {
1355         struct at_sync_request *sync_request = NULL;
1356         struct at_sync_request **sync_requests = NULL;
1357         int sync_requests_count = 0;
1358         int i;
1359
1360         if(command == NULL)
1361                 return NULL;
1362
1363         AT_SYNC_LOCK();
1364
1365         // Save the previous data pointer and count
1366         sync_requests = at_handling.sync_requests_queue.sync_requests;
1367         sync_requests_count = at_handling.sync_requests_queue.sync_requests_count;
1368
1369         if(sync_requests_count <= 0 || sync_requests == NULL) {
1370                 AT_SYNC_UNLOCK();
1371                 return NULL;
1372         }
1373
1374         for(i=0 ; i < sync_requests_count ; i++) {
1375                 if(sync_requests[i] != NULL && sync_requests[i]->command != NULL) {
1376                         if(at_commands_compare(command, sync_requests[i]->command)) {
1377                                 sync_request = sync_requests[i];
1378                                 break;
1379                         }
1380                 }
1381         }
1382
1383         AT_SYNC_UNLOCK();
1384
1385         return sync_request;
1386 }
1387
1388 // Find a sync request from its handled status
1389 struct at_sync_request *at_sync_request_find_handled(int handled)
1390 {
1391         struct at_sync_request *sync_request = NULL;
1392         struct at_sync_request **sync_requests = NULL;
1393         int sync_requests_count = 0;
1394         int i;
1395
1396         AT_SYNC_LOCK();
1397
1398         // Save the previous data pointer and count
1399         sync_requests = at_handling.sync_requests_queue.sync_requests;
1400         sync_requests_count = at_handling.sync_requests_queue.sync_requests_count;
1401
1402         if(sync_requests_count <= 0 || sync_requests == NULL) {
1403                 AT_SYNC_UNLOCK();
1404                 return NULL;
1405         }
1406
1407         for(i=0 ; i < sync_requests_count ; i++) {
1408                 if(sync_requests[i] != NULL) {
1409                         if(sync_requests[i]->handled == handled) {
1410                                 sync_request = sync_requests[i];
1411                                 break;
1412                         }
1413                 }
1414         }
1415
1416         AT_SYNC_UNLOCK();
1417
1418         return sync_request;
1419 }
1420
1421 // Find a sync request from its request pointer
1422 struct at_sync_request *at_sync_request_find_request(struct at_request *request)
1423 {
1424         struct at_sync_request *sync_request = NULL;
1425         struct at_sync_request **sync_requests = NULL;
1426         int sync_requests_count = 0;
1427         int i;
1428
1429         if(request == NULL)
1430                 return NULL;
1431
1432         AT_SYNC_LOCK();
1433
1434         // Save the previous data pointer and count
1435         sync_requests = at_handling.sync_requests_queue.sync_requests;
1436         sync_requests_count = at_handling.sync_requests_queue.sync_requests_count;
1437
1438         if(sync_requests_count <= 0 || sync_requests == NULL) {
1439                 AT_SYNC_UNLOCK();
1440                 return NULL;
1441         }
1442
1443         for(i=0 ; i < sync_requests_count ; i++) {
1444                 if(sync_requests[i] != NULL) {
1445                         if(sync_requests[i]->request == request) {
1446                                 sync_request = sync_requests[i];
1447                                 break;
1448                         }
1449                 }
1450         }
1451
1452         AT_SYNC_UNLOCK();
1453
1454         return sync_request;
1455 }
1456
1457 // Queue one request to the sync requests queue
1458 int at_sync_request_queue(struct at_sync_request *sync_request)
1459 {
1460         struct at_sync_request **sync_requests = NULL;
1461         int sync_requests_count = 0;
1462         int index;
1463         int count;
1464
1465         if(sync_request == NULL)
1466                 return -1;
1467
1468         AT_SYNC_LOCK();
1469
1470         // Save the previous data pointer and count
1471         sync_requests = at_handling.sync_requests_queue.sync_requests;
1472         sync_requests_count = at_handling.sync_requests_queue.sync_requests_count;
1473
1474         if(sync_requests_count < 0)
1475                 sync_requests_count = 0;
1476
1477         // Index is the sync request index in the sync requests array
1478         index = sync_requests_count;
1479         // Count is the total count of sync requests in the array
1480         count = index + 1;
1481
1482         // Alloc the array with the new size
1483         at_handling.sync_requests_queue.sync_requests = malloc(sizeof(struct at_sync_request *) * count);
1484         at_handling.sync_requests_queue.sync_requests_count = count;
1485
1486         // Copy and free previous data
1487         if(sync_requests != NULL && sync_requests_count > 0) {
1488                 memcpy(at_handling.sync_requests_queue.sync_requests, sync_requests, sizeof(struct at_sync_request *) * at_handling.sync_requests_queue.sync_requests_count);
1489                 free(sync_requests);
1490         }
1491
1492         // Get the new data pointer and count
1493         sync_requests = at_handling.sync_requests_queue.sync_requests;
1494         sync_requests_count = at_handling.sync_requests_queue.sync_requests_count;
1495
1496         // Put the sync request in the queue
1497         sync_requests[index] = sync_request;
1498
1499         LOGD("%d elements in the sync requests queue", sync_requests_count);
1500
1501         AT_SYNC_UNLOCK();
1502
1503         return 0;
1504 }
1505
1506 // Dequeue one response (that will or not be assigned to a sync request)
1507 int at_sync_response_dequeue(struct at_response *response)
1508 {
1509         struct at_sync_request *sync_request = NULL;
1510         struct at_async_request *async_request = NULL;
1511         int rc;
1512
1513         if(response == NULL)
1514                 return -1;
1515
1516         if(response->command != NULL && !at_sync_queue_empty()) {
1517                 if(response->status == AT_STATUS_UNDEF && (response->data == NULL || response->data_count <= 0)) {
1518                         // Check if this is a part of a queued sync command
1519
1520                         sync_request = at_sync_request_find_command(response->command);
1521                         if(sync_request != NULL && sync_request->handled == AT_RESPONSE_SENT) {
1522                                 // This will make this sync request the next one to get a status
1523
1524                                 sync_request->handled = AT_RESPONSE_UNHANDELD_REASON_STATUS;
1525
1526                                 LOGD("Skipping matching request with no status nor data!");
1527                                 at_response_free(response);
1528                                 return 0;
1529                         }
1530                 }
1531
1532                 // FIXME: What if we get some data for the sync request here, but not with the status yet => set response already => next time if there is already a response, grab its data
1533
1534                 sync_request = at_sync_request_find_handled(AT_RESPONSE_SENT);
1535                 if(sync_request == NULL)
1536                         sync_request = at_sync_request_find_handled(AT_RESPONSE_UNHANDELD_REASON_STATUS);
1537
1538                 if(sync_request != NULL && sync_request->command != NULL) {
1539                         // If we catch an unsol or a response for another request, there is something wrong
1540
1541                         rc = at_commands_compare(response->command, sync_request->command);
1542                         if(!rc) {
1543                                 // We don't need to check on the async queue as requests are sent one at a time
1544
1545                                 LOGE("Got a response for another request, aborting!");
1546                                 AT_SYNC_QUEUE_UNLOCK();
1547                                 return -1;
1548                         }
1549                 } else {
1550                         // There is a command but we didn't send any sync request, so don't deal with it
1551
1552                         return -1;
1553                 }
1554         }
1555
1556         // If we have no status at this point, we have no use of the response
1557         if(response->status == AT_STATUS_UNDEF) {
1558                 return -1;
1559         }
1560
1561         // Have no command but a status
1562         if(response->command == NULL) {
1563                 async_request = at_async_request_find_handled(AT_RESPONSE_UNHANDELD_REASON_STATUS);
1564                 if(async_request != NULL) {
1565                         // FIXME: What if there was already a response with data but no status? We would override data => make a mix of both and free one
1566
1567                         async_request->response = response;
1568                         async_request->handled = AT_RESPONSE_SENT;
1569
1570                         LOGD("Found a status for a previous async request!");
1571
1572                         return -1;
1573                 }
1574
1575                 sync_request = at_sync_request_find_handled(AT_RESPONSE_UNHANDELD_REASON_STATUS);
1576                 if(sync_request != NULL) {
1577                         sync_request->handled = AT_RESPONSE_SENT;
1578                 }
1579
1580         }
1581
1582         if(sync_request == NULL) {
1583                 sync_request = at_sync_request_find_handled(AT_RESPONSE_SENT);
1584                 if(sync_request == NULL) {
1585                         LOGE("Got a status but no request waiting for it at all, this should never happend!");
1586                         return -1;
1587                 }
1588         }
1589
1590         sync_request->response = response;
1591         LOGD("Found a response for a sync request!");
1592
1593         AT_SYNC_QUEUE_UNLOCK();
1594
1595         return 0;
1596 }
1597
1598 // Dequeue one sync request (only remove from the queue)
1599 int at_sync_request_dequeue(struct at_sync_request *sync_request)
1600 {
1601         struct at_sync_request **sync_requests = NULL;
1602         int sync_requests_count = 0;
1603         int pos = -1;
1604         int i;
1605
1606         if(sync_request == NULL)
1607                 return -1;
1608
1609         AT_SYNC_LOCK();
1610
1611         // Save the previous data pointer and count
1612         sync_requests = at_handling.sync_requests_queue.sync_requests;
1613         sync_requests_count = at_handling.sync_requests_queue.sync_requests_count;
1614
1615         if(sync_requests_count <= 0 || sync_requests == NULL) {
1616                 LOGE("No sync requests queued, aborting dequeue!");
1617
1618                 AT_SYNC_UNLOCK();
1619                 return -1;
1620         }
1621
1622         for(i=0 ; i < sync_requests_count ; i++) {
1623                 if(sync_requests[i] == sync_request) {
1624                         pos = i;
1625                         break;
1626                 }
1627         }
1628
1629         if(pos < 0) {
1630                 LOGD("Found no matching sync request, aborting dequeue!");
1631
1632                 AT_SYNC_UNLOCK();
1633                 return -1;
1634         }
1635
1636         // Empty the found position in the sync requests array
1637         sync_requests[pos] = NULL;
1638
1639         // Move the elements back
1640         for(i=pos ; i < sync_requests_count-1 ; i++) {
1641                 sync_requests[i] = sync_requests[i+1];
1642         }
1643
1644         // Empty the latest element
1645         if(pos != sync_requests_count-1) {
1646                 sync_requests[sync_requests_count-1] = NULL;
1647         }
1648
1649         sync_requests_count--;
1650
1651         if(sync_requests_count == 0) {
1652                 free(sync_requests);
1653                 at_handling.sync_requests_queue.sync_requests = NULL;
1654         }
1655
1656         at_handling.sync_requests_queue.sync_requests_count = sync_requests_count;
1657
1658         LOGD("%d elements in the sync requests queue", sync_requests_count);
1659
1660         AT_SYNC_UNLOCK();
1661
1662         return 0;
1663 }
1664
1665 /*
1666  * Freeze
1667  */
1668
1669 void at_freeze_set(int freeze)
1670 {
1671         at_handling.freeze = freeze;
1672 }
1673
1674 int at_freeze_get(void)
1675 {
1676         return at_handling.freeze;
1677 }
1678
1679 int at_freeze_start(void)
1680 {
1681         struct at_sync_request *sync_request = NULL;
1682         struct at_async_request *async_request = NULL;
1683
1684         at_freeze_set(AT_FREEZE_ON);
1685
1686         // Set all the sent requests to freezed state
1687         sync_request = at_sync_request_find_handled(AT_RESPONSE_SENT);
1688         if(sync_request)
1689                 sync_request->handled = AT_RESPONSE_SENT_FREEZED;
1690
1691         async_request = at_async_request_find_handled(AT_RESPONSE_SENT);
1692         if(async_request)
1693                 async_request->handled = AT_RESPONSE_SENT_FREEZED;
1694
1695         async_request = at_async_request_find_handled(AT_RESPONSE_UNHANDELD_REASON_STATUS);
1696         if(async_request)
1697                 async_request->handled = AT_RESPONSE_SENT_FREEZED;
1698
1699         async_request = at_async_request_find_handled(AT_RESPONSE_UNHANDELD_REASON_DATA);
1700         if(async_request)
1701                 async_request->handled = AT_RESPONSE_SENT_FREEZED;
1702
1703         return 0;
1704 }
1705
1706 int at_freeze_stop(void)
1707 {
1708         at_freeze_set(AT_FREEZE_OFF);
1709
1710         return 0;
1711 }
1712
1713 int at_freeze_send(void)
1714 {
1715         // Allow requests send even though there are freezed requests
1716         at_freeze_set(AT_FREEZE_SEND);
1717
1718         // Unlock responses queue so that the dispatch loop can send setup
1719         AT_RESPONSES_QUEUE_UNLOCK();
1720
1721         return 0;
1722 }
1723
1724 int at_freeze_respawn(void)
1725 {
1726         struct at_sync_request *sync_request = NULL;
1727         struct at_async_request *async_request = NULL;
1728         int rc;
1729
1730         // Respawn the latest request
1731         at_freeze_set(AT_FREEZE_RESPAWN);
1732
1733         // There should only be one
1734         do {
1735                 sync_request = at_sync_request_find_handled(AT_RESPONSE_SENT_FREEZED);
1736                 if(sync_request) {
1737                         sync_request->handled = AT_RESPONSE_WAITING;
1738
1739                         rc = at_request_queue(sync_request->request);
1740                         if(rc < 0) {
1741                                 LOGE("Unable to queue request");
1742
1743                                 // Better trying to dequeue too
1744                                 at_sync_request_dequeue(sync_request);
1745                                 at_sync_request_free(sync_request);
1746                         } else {
1747                                 at_send_next_request();
1748                         }
1749                 }
1750         } while(sync_request != NULL);
1751
1752         do {
1753                 async_request = at_async_request_find_handled(AT_RESPONSE_SENT_FREEZED);
1754                 if(async_request) {
1755                         async_request->handled = AT_RESPONSE_WAITING;
1756
1757                         rc = at_request_queue(async_request->request);
1758                         if(rc < 0) {
1759                                 LOGE("Unable to queue request");
1760
1761                                 // Better trying to dequeue too
1762                                 at_async_request_dequeue(async_request);
1763                                 at_async_request_free(async_request);
1764                         } else {
1765                                 at_send_next_request();
1766                         }
1767                 }
1768         } while(async_request != NULL);
1769
1770         return 0;
1771 }
1772
1773 /*
1774  * Request
1775  */
1776
1777 int at_request_send(struct at_request *request)
1778 {
1779         char *string = NULL;
1780         int length = 0;
1781
1782         int offset_separator_begin = 0;
1783         int length_separator_begin = 0;
1784         int offset_command = 0;
1785         int length_command = 0;
1786         int offset_data_separator = 0;
1787         int length_data_separator = 0;
1788         int offset_data = 0;
1789         int length_data = 0;
1790         int offset_separator_end = 0;
1791         int length_separator_end = 0;
1792
1793         int i, p;
1794
1795         // TODO: Do we send \r\n or only \r, begining/end or just end?
1796         char separator[] = "\r\n";
1797         char data_separator[] = "=";
1798
1799         if(request->command == NULL)
1800                 return -1;
1801
1802         offset_separator_begin = 0;
1803         length_separator_begin = strlen(separator);
1804         length += length_separator_begin;
1805
1806         offset_command = length;
1807         length_command = strlen(request->command);
1808         length += length_command;
1809
1810         if(request->data != NULL) {
1811                 offset_data_separator = length;
1812                 length_data_separator = strlen(data_separator);
1813                 length += length_data_separator;
1814
1815                 offset_data = length;
1816                 length_data = strlen(request->data);
1817                 length += length_data;
1818         }
1819
1820         offset_separator_end = length;
1821         length_separator_end = strlen(separator);
1822         length += length_separator_end;
1823
1824         // Alloc final string
1825         string = calloc(1, length);
1826
1827         // Copy the data to the string
1828         memcpy(string + offset_separator_begin, separator, length_separator_begin);
1829         memcpy(string + offset_command, request->command, length_command);
1830         if(request->data != NULL) {
1831                 memcpy(string + offset_data_separator, data_separator, length_data_separator);
1832                 memcpy(string + offset_data, request->data, length_data);
1833         }
1834         memcpy(string + offset_separator_end, separator, length_separator_end);
1835
1836         // Log request
1837         RIL_LOG_LOCK();
1838         ril_data_log(string, length);
1839         ril_send_log(request);
1840         RIL_LOG_UNLOCK();
1841
1842         ril_device_transport_send(ril_device, string, length);
1843
1844         free(string);
1845
1846         return 0;
1847 }
1848
1849 struct at_request *at_request_create(char *command, char *data)
1850 {
1851         struct at_request *request = NULL;
1852         int i;
1853
1854         if(command == NULL)
1855                 return NULL;
1856
1857         request = calloc(1, sizeof(struct at_request));
1858
1859         asprintf(&(request->command), "%s", command);
1860
1861         if(data != NULL) {
1862                 asprintf(&(request->data), "%s", data);
1863         }
1864
1865         return request;
1866 }
1867
1868 void at_request_free(struct at_request *request)
1869 {
1870         if(request->command != NULL) 
1871                 free(request->command);
1872
1873         if(request->data != NULL)
1874                 free(request->data);
1875
1876         if(request->error != NULL)
1877                 free(request->error);
1878
1879         memset(request, 0, sizeof(struct at_request *));
1880 }
1881
1882 int at_request_send_async(struct at_request *request, void *data, RIL_Token token, at_async_request_cb func)
1883 {
1884         struct at_async_request *async_request = NULL;
1885         int rc;
1886         int i;
1887
1888         if(request->command == NULL || func == NULL)
1889                 return -1;
1890
1891         async_request = calloc(1, sizeof(struct at_async_request));
1892         async_request->handled = AT_RESPONSE_WAITING;
1893         async_request->request = request;
1894         async_request->command = strdup(request->command);
1895         async_request->data = data;
1896         async_request->token = token;
1897         async_request->func = func;
1898
1899         if(at_freeze_get() == AT_FREEZE_SEND)
1900                 async_request->handled = AT_RESPONSE_UNFREEZE;
1901
1902         rc = at_async_request_queue(async_request);
1903         if(rc < 0) {
1904                 LOGE("Unable to queue async request");
1905
1906                 at_async_request_free(async_request);
1907                 return -1;
1908         }
1909
1910         // Do not queue the request when sending while freeze
1911         if(at_freeze_get() == AT_FREEZE_SEND)
1912                 goto request_send;
1913
1914         rc = at_request_queue(request);
1915         if(rc < 0) {
1916                 LOGE("Unable to queue request");
1917
1918                 // Better trying to dequeue too
1919                 at_async_request_dequeue(async_request);
1920                 at_async_request_free(async_request);
1921                 return NULL;
1922         }
1923
1924 request_send:
1925         // Try to send it now, don't fail if it can't
1926         at_send_next_request();
1927
1928         return 0;
1929 }
1930
1931 struct at_response *at_request_send_sync(struct at_request *request)
1932 {
1933         struct at_sync_request *sync_request = NULL;
1934         struct at_response *response = NULL;
1935         int rc;
1936         int i;
1937
1938         if(request->command == NULL)
1939                 return NULL;
1940
1941         sync_request = calloc(1, sizeof(struct at_sync_request));
1942         sync_request->handled = AT_RESPONSE_WAITING;
1943         sync_request->request = request;
1944         sync_request->command = strdup(request->command);
1945
1946         if(at_freeze_get() == AT_FREEZE_SEND)
1947                 sync_request->handled = AT_RESPONSE_UNFREEZE;
1948
1949         rc = at_sync_request_queue(sync_request);
1950         if(rc < 0) {
1951                 LOGE("Unable to queue sync request");
1952
1953                 at_sync_request_free(sync_request);
1954                 return NULL;
1955         }
1956
1957         // Do not queue the request when sending while freeze
1958         if(at_freeze_get() == AT_FREEZE_SEND)
1959                 goto request_send;
1960
1961         rc = at_request_queue(request);
1962         if(rc < 0) {
1963                 LOGE("Unable to queue request");
1964
1965                 // Better trying to dequeue too
1966                 at_sync_request_dequeue(sync_request);
1967                 at_sync_request_free(sync_request);
1968                 return NULL;
1969         }
1970
1971 request_send:
1972         // Try to send it now, don't fail if it can't
1973         at_send_next_request();
1974
1975         // Block until there is a response available
1976         at_sync_requests_queue_lock();
1977
1978         if(sync_request->response == NULL) {
1979                 LOGE("Sync queue was unlocked but there is no response, aborting");
1980
1981                 // Better trying to dequeue too
1982                 at_sync_request_dequeue(sync_request);
1983                 at_sync_request_free(sync_request);
1984
1985                 // Send the next request in the queue
1986                 at_send_next_request();
1987
1988                 return NULL;
1989         }
1990
1991         response = sync_request->response;
1992
1993         at_sync_request_dequeue(sync_request);
1994         at_sync_request_free(sync_request);
1995
1996         // Send the next request in the queue
1997         at_send_next_request();
1998
1999         return response;
2000 }
2001
2002 /*
2003  * Send
2004  */
2005
2006 int at_send_next_request(void)
2007 {
2008         struct at_sync_request *sync_request = NULL;
2009         struct at_async_request *async_request = NULL;
2010         struct at_async_request *async_request_sent = NULL;
2011         struct at_async_request *async_request_unhandled_status = NULL;
2012         struct at_async_request *async_request_unhandled_data = NULL;
2013         struct at_request *request = NULL;
2014         int rc;
2015
2016         if(at_freeze_get() == AT_FREEZE_SEND) {
2017                 sync_request = at_sync_request_find_handled(AT_RESPONSE_UNFREEZE);
2018                 if(sync_request != NULL) {
2019                         request = sync_request->request;
2020                         goto request_send;
2021                 }
2022
2023                 async_request = at_async_request_find_handled(AT_RESPONSE_UNFREEZE);
2024                 if(async_request != NULL) {
2025                         request = async_request->request;
2026                         goto request_send;
2027                 }
2028
2029                 if(request == NULL) {
2030                         LOGD("No request to send during freeze!");
2031                         return -1;
2032                 }
2033         }
2034
2035         // Unhandled requests are still going on too
2036         sync_request = at_sync_request_find_handled(AT_RESPONSE_SENT);
2037         async_request_sent = at_async_request_find_handled(AT_RESPONSE_SENT);
2038         async_request_unhandled_status = at_async_request_find_handled(AT_RESPONSE_UNHANDELD_REASON_STATUS);
2039         async_request_unhandled_data = at_async_request_find_handled(AT_RESPONSE_UNHANDELD_REASON_DATA);
2040
2041         if(sync_request != NULL || async_request_sent != NULL || async_request_unhandled_status != NULL || async_request_unhandled_data != NULL) {
2042                 LOGE("There are still unanswered requests!");
2043                 return -1;
2044         }
2045
2046         request = at_request_dequeue();
2047         if(request == NULL) {
2048                 LOGD("Nothing left to send!");
2049                 return -1;
2050         }
2051
2052 request_send:
2053         rc = at_request_send(request);
2054         if(rc < 0) {
2055                 LOGE("Unable to send request, aborting!");
2056                 at_request_free(request);
2057                 return -1;
2058         }
2059
2060         sync_request = at_sync_request_find_request(request);
2061         if(sync_request != NULL)
2062                 sync_request->handled = AT_RESPONSE_SENT;
2063
2064         async_request = at_async_request_find_request(request);
2065         if(async_request != NULL)
2066                 async_request->handled = AT_RESPONSE_SENT;
2067
2068         return 0;
2069 }
2070
2071 struct at_response *at_send_sync(char *command, char *data)
2072 {
2073         struct at_request *request = NULL;
2074         struct at_response *response = NULL;
2075
2076         request = at_request_create(command, data);
2077         if(request == NULL) {
2078                 LOGE("Unable to create request, aborting");
2079                 return -1;
2080         }
2081
2082         response = at_request_send_sync(request);
2083         if(response == NULL) {
2084                 at_request_free(request);
2085                 return NULL;
2086         }
2087
2088         return response;
2089 }
2090
2091 int at_send_expect_to_func(char *command, char *data, void *async_data, RIL_Token token, at_async_request_cb func)
2092 {
2093         struct at_request *request = NULL;
2094         int rc;
2095
2096         request = at_request_create(command, data);
2097         if(request == NULL) {
2098                 LOGE("Unable to create request, aborting");
2099                 return -1;
2100         }
2101
2102         rc = at_request_send_async(request, async_data, token, func);
2103         if(rc < 0) {
2104                 LOGE("Unable to send async request, aborting");
2105                 at_request_free(request);
2106                 return -1;
2107         }
2108
2109         return 0;
2110 }
2111
2112 int at_send_expect_status(char *command, char *data)
2113 {
2114         struct at_response *response = NULL;
2115         int status;
2116
2117         response = at_send_sync(command, data);
2118         if(response == NULL) {
2119                 LOGE("Unable to get sync response, aborting");
2120                 return -1;
2121         }
2122
2123         status = response->status;
2124
2125         at_response_free(response);
2126
2127         return status;
2128 }
2129
2130 int at_send_noresp(char *command, char *data)
2131 {
2132         struct at_response *response;
2133
2134         response = at_send(command, data);
2135         if(response != NULL) {
2136                 at_response_free(response);
2137                 return 0;
2138         } else {
2139                 return -1;
2140         }
2141 }
2142
2143 struct at_response *at_send_command(char *command)
2144 {
2145         return at_send(command, NULL);
2146 }
2147
2148 struct at_response *at_send(char *command, char *data)
2149 {
2150         struct at_request *request = NULL;
2151         struct at_response *response = NULL;
2152         int rc;
2153
2154         request = at_request_create(command, data);
2155         if(request == NULL) {
2156                 LOGE("Unable to create request, aborting!");
2157                 return -1;
2158         }
2159
2160         response = at_request_send_sync(request);
2161         if(response == NULL) {
2162                 LOGE("Unable to get sync response, aborting");
2163
2164                 at_request_free(request);
2165                 return -1;
2166         }
2167
2168         return response;
2169 }