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 }