commands-wms.c

   1 /*
   2  * uqmi -- tiny QMI support implementation
   3  *
   4  * Copyright (C) 2014-2015 Felix Fietkau <nbd@openwrt.org>
   5  *
   6  * This library is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public
   8  * License as published by the Free Software Foundation; either
   9  * version 2 of the License, or (at your option) any later version.
  10  *
  11  * This library is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14  * Lesser General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU Lesser General Public
  17  * License along with this library; if not, write to the
  18  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  19  * Boston, MA 02110-1301 USA.
  20  */
  21
  22 #include "qmi-message.h"
  23
  24 #define MIN(a,b) (((a)<(b))?(a):(b))
  25 #define CEILDIV(x,y) (((x) + (y) - 1) / (y))
  26
  27 static struct qmi_wms_list_messages_request lmreq = {
  28         QMI_INIT(storage_type, QMI_WMS_STORAGE_TYPE_UIM),
  29         QMI_INIT(message_tag, QMI_WMS_MESSAGE_TAG_TYPE_MT_NOT_READ),
  30 };
  31
  32 static struct qmi_wms_delete_request dmreq = {
  33         QMI_INIT(memory_storage, QMI_WMS_STORAGE_TYPE_UIM),
  34         QMI_INIT(message_mode, QMI_WMS_MESSAGE_MODE_GSM_WCDMA),
  35 };
  36
  37 static struct qmi_wms_raw_read_request gmreq = {
  38         QMI_INIT_SEQUENCE(message_memory_storage_id,
  39                 .storage_type = QMI_WMS_STORAGE_TYPE_UIM,
  40         ),
  41         QMI_INIT(message_mode, QMI_WMS_MESSAGE_MODE_GSM_WCDMA),
  42 };
  43
  44
  45 #define cmd_wms_storage_cb no_cb
  46 static enum qmi_cmd_result
  47 cmd_wms_storage_prepare(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, char *arg)
  48 {
  49         if (strcmp(arg, "sim") == 0) {
  50         } else if (strcmp(arg, "me") == 0) {
  51                 qmi_set_ptr(&lmreq, storage_type, QMI_WMS_STORAGE_TYPE_NV);
  52                 qmi_set_ptr(&dmreq, memory_storage, QMI_WMS_STORAGE_TYPE_NV);
  53                 qmi_set_ptr(&gmreq, message_memory_storage_id.storage_type, QMI_WMS_STORAGE_TYPE_NV);
  54         } else {
  55                 uqmi_add_error("Invalid value (sim or me)");
  56                 return QMI_CMD_EXIT;
  57         }
  58         return QMI_CMD_DONE;
  59 }
  60
  61 static void cmd_wms_list_messages_cb(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg)
  62 {
  63         struct qmi_wms_list_messages_response res;
  64         void *c;
  65         int i;
  66
  67         qmi_parse_wms_list_messages_response(msg, &res);
  68         c = blobmsg_open_array(&status, NULL);
  69         for (i = 0; i < res.data.message_list_n; i++)
  70                 blobmsg_add_u32(&status, NULL, res.data.message_list[i].memory_index);
  71
  72         blobmsg_close_array(&status, c);
  73 }
  74
  75 static enum qmi_cmd_result
  76 cmd_wms_list_messages_prepare(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, char *arg)
  77 {
  78         qmi_set_wms_list_messages_request(msg, &lmreq);
  79
  80         return QMI_CMD_REQUEST;
  81 }
  82
  83 static int
  84 put_unicode_char(char *dest, uint16_t c)
  85 {
  86         if (c < 0x80) {
  87                 *dest = c;
  88                 return 1;
  89         } else if (c < 0x800) {
  90                 *(dest++) = 0xc0 | ((c >> 6) & 0x1f);
  91                 *dest = 0x80 | (c & 0x3f);
  92                 return 2;
  93         } else {
  94                 *(dest++) = 0xe0 | ((c >> 12) & 0xf);
  95                 *(dest++) = 0x80 | ((c >> 6) & 0x3f);
  96                 *dest = 0x80 | (c & 0x3f);
  97                 return 3;
  98         }
  99 }
 100
 101
 102 static int
 103 pdu_decode_7bit_char(char *dest, int len, unsigned char c, bool *escape)
 104 {
 105         uint16_t conv_0x20[] = {
 106                 0x0040, 0x00A3, 0x0024, 0x00A5, 0x00E8, 0x00E9, 0x00F9, 0x00EC,
 107                 0x00F2, 0x00E7, 0x000A, 0x00D8, 0x00F8, 0x000D, 0x00C5, 0x00E5,
 108                 0x0394, 0x005F, 0x03A6, 0x0393, 0x039B, 0x03A9, 0x03A0, 0x03A8,
 109                 0x03A3, 0x0398, 0x039E, 0x00A0, 0x00C6, 0x00E6, 0x00DF, 0x00C9,
 110         };
 111         uint16_t conv_0x5b[] = {
 112                 0x00C4, 0x00D6, 0x00D1, 0x00DC, 0x00A7, 0x00BF,
 113         };
 114         uint16_t conv_0x7b[] = {
 115                 0x00E4, 0x00F6, 0x00F1, 0x00FC, 0x00E0
 116         };
 117         int cur_len = 0;
 118         uint16_t outc;
 119
 120         fprintf(stderr, " %02x", c);
 121         dest += len;
 122         if (*escape) {
 123                 *escape = false;
 124                 switch(c) {
 125                 case 0x0A:
 126                         *dest = 0x0C;
 127                         return 1;
 128                 case 0x14:
 129                         *dest = 0x5E;
 130                         return 1;
 131                 case 0x28:
 132                         *dest = 0x7B;
 133                         return 1;
 134                 case 0x29:
 135                         *dest = 0x7D;
 136                         return 1;
 137                 case 0x2F:
 138                         *dest = 0x5C;
 139                         return 1;
 140                 case 0x3C:
 141                         *dest = 0x5B;
 142                         return 1;
 143                 case 0x3D:
 144                         *dest = 0x7E;
 145                         return 1;
 146                 case 0x3E:
 147                         *dest = 0x5D;
 148                         return 1;
 149                 case 0x40:
 150                         *dest = 0x7C;
 151                         return 1;
 152                 case 0x65:
 153                         outc = 0x20AC;
 154                         goto out;
 155                 case 0x1B:
 156                         goto normal;
 157                 default:
 158                         /* invalid */
 159                         *(dest++) = conv_0x20[0x1B];
 160                         cur_len++;
 161                         goto normal;
 162                 }
 163         }
 164
 165         if (c == 0x1b) {
 166                 *escape = true;
 167                 return 0;
 168         }
 169
 170 normal:
 171         if (c < 0x20)
 172                 outc = conv_0x20[(int) c];
 173         else if (c == 0x40)
 174                 outc = 0x00A1;
 175         else if (c >= 0x5b && c <= 0x60)
 176                 outc = conv_0x5b[c - 0x5b];
 177         else if (c >= 0x7b && c <= 0x7f)
 178                 outc = conv_0x7b[c - 0x7b];
 179         else
 180                 outc = c;
 181
 182 out:
 183         return cur_len + put_unicode_char(dest, outc);
 184 }
 185
 186 static int
 187 pdu_decode_7bit_str(char *dest, const unsigned char *data, int data_len, int bit_offset)
 188 {
 189         bool escape = false;
 190         int len = 0;
 191         int i;
 192
 193         fprintf(stderr, "Raw text:");
 194         for (i = 0; i < data_len; i++) {
 195                 int pos = (i + bit_offset) % 7;
 196
 197                 if (pos == 0) {
 198                         len += pdu_decode_7bit_char(dest, len, data[i] & 0x7f, &escape);
 199                 } else {
 200                         if (i)
 201                                 len += pdu_decode_7bit_char(dest, len,
 202                                                             (data[i - 1] >> (7 + 1 - pos)) |
 203                                                             ((data[i] << pos) & 0x7f), &escape);
 204
 205                         if (pos == 6)
 206                                 len += pdu_decode_7bit_char(dest, len, (data[i] >> 1) & 0x7f,
 207                                                             &escape);
 208                 }
 209         }
 210         dest[len] = 0;
 211         fprintf(stderr, "\n");
 212         return len;
 213 }
 214
 215 static int decode_udh(const unsigned char *data)
 216 {
 217         const unsigned char *end;
 218         unsigned int type, len, udh_len;
 219
 220         udh_len = *(data++);
 221         end = data + udh_len;
 222         while (data < end) {
 223                 const unsigned char *val;
 224
 225                 type = data[0];
 226                 len = data[1];
 227                 val = &data[2];
 228                 data += 2 + len;
 229                 if (data > end)
 230                         break;
 231
 232                 switch (type) {
 233                 case 0x00:
 234                         blobmsg_add_u32(&status, "concat_ref", (uint32_t) val[0]);
 235                         blobmsg_add_u32(&status, "concat_part", (uint32_t) val[2]);
 236                         blobmsg_add_u32(&status, "concat_parts", (uint32_t) val[1]);
 237                         break;
 238                 case 0x08:
 239                         blobmsg_add_u32(&status, "concat_ref", (uint32_t) (val[0] << 8 | val[1]));
 240                         blobmsg_add_u32(&status, "concat_part", (uint32_t) val[3]);
 241                         blobmsg_add_u32(&status, "concat_parts", (uint32_t) val[2]);
 242                         break;
 243                 default:
 244                         break;
 245                 }
 246         }
 247
 248         return udh_len + 1;
 249 }
 250
 251 static void decode_7bit_field(char *name, const unsigned char *data, int data_len, int bit_offset)
 252 {
 253         char *dest = blobmsg_alloc_string_buffer(&status, name, 3 * data_len + 2);
 254         int out_len = pdu_decode_7bit_str(dest, data, CEILDIV(data_len * 7, 8), bit_offset);
 255         dest[out_len] = 0;
 256         blobmsg_add_string_buffer(&status);
 257 }
 258
 259 static char *pdu_add_semioctet(char *str, char val)
 260 {
 261         *str = '0' + (val & 0xf);
 262         if (*str <= '9')
 263                 str++;
 264
 265         *str = '0' + ((val >> 4) & 0xf);
 266         if (*str <= '9')
 267                 str++;
 268
 269         return str;
 270 }
 271
 272 static void
 273 pdu_decode_address(char *str, unsigned char *data, int len)
 274 {
 275         unsigned char toa;
 276
 277         toa = *(data++);
 278         switch (toa & 0x70) {
 279         case 0x50:
 280                 pdu_decode_7bit_str(str, data, len, 0);
 281                 return;
 282         case 0x10:
 283                 *(str++) = '+';
 284                 /* fall through */
 285         default:
 286                 while (len--) {
 287                         str = pdu_add_semioctet(str, *data);
 288                         data++;
 289                 }
 290         }
 291
 292         *str = 0;
 293 }
 294
 295 static void wms_decode_address(char *name, unsigned char *data, int len)
 296 {
 297         char *str = blobmsg_alloc_string_buffer(&status, name, len * 2 + 2);
 298         pdu_decode_address(str, data, len);
 299         blobmsg_add_string_buffer(&status);
 300 }
 301
 302 static void blobmsg_add_hex(struct blob_buf *buf, const char *name, unsigned const char *data, int len)
 303 {
 304         char* str = blobmsg_alloc_string_buffer(buf, name, len * 2 + 1);
 305         for (int i = 0; i < len; i++) {
 306                 str += sprintf(str, "%02x", data[i]);
 307         }
 308         blobmsg_add_string_buffer(buf);
 309 }
 310
 311 #define cmd_wms_delete_message_cb no_cb
 312 static enum qmi_cmd_result
 313 cmd_wms_delete_message_prepare(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, char *arg)
 314 {
 315         char *err;
 316         int id;
 317
 318         id = strtoul(arg, &err, 10);
 319         if (err && *err) {
 320                 uqmi_add_error("Invalid message ID");
 321                 return QMI_CMD_EXIT;
 322         }
 323
 324         dmreq.set.memory_index = 1;
 325         dmreq.data.memory_index = id;
 326
 327         qmi_set_wms_delete_request(msg, &dmreq);
 328
 329         return QMI_CMD_REQUEST;
 330 }
 331
 332
 333 static void cmd_wms_get_message_cb(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg)
 334 {
 335         struct qmi_wms_raw_read_response res;
 336         unsigned char *data, *end;
 337         char *str;
 338         int cur_len;
 339         bool sent;
 340         unsigned char first, dcs;
 341         void *c;
 342
 343         qmi_parse_wms_raw_read_response(msg, &res);
 344         c = blobmsg_open_table(&status, NULL);
 345         data = (unsigned char *) res.data.raw_message_data.raw_data;
 346         end = data + res.data.raw_message_data.raw_data_n;
 347
 348         cur_len = *(data++);
 349         if (data + cur_len >= end)
 350                 goto error;
 351
 352         if (cur_len) {
 353                 wms_decode_address("smsc", data, cur_len - 1);
 354                 data += cur_len;
 355         }
 356
 357         if (data + 3 >= end)
 358                 goto error;
 359
 360         first = *(data++);
 361         sent = (first & 0x3) == 1;
 362         if (sent)
 363                 data++;
 364
 365         cur_len = *(data++);
 366         if (data + cur_len >= end)
 367                 goto error;
 368
 369         if (cur_len) {
 370                 cur_len = (cur_len + 1) / 2;
 371                 wms_decode_address(sent ? "receiver" : "sender", data, cur_len);
 372                 data += cur_len + 1;
 373         }
 374
 375         if (data + 3 >= end)
 376                 goto error;
 377
 378         /* Protocol ID */
 379         if (*(data++) != 0)
 380                 goto error;
 381
 382         /* Data Encoding */
 383         dcs = *(data++);
 384
 385         if (dcs & 0x10)
 386                 blobmsg_add_u32(&status, "class", (dcs & 3));
 387
 388         if (sent) {
 389                 /* Message validity */
 390                 data++;
 391         } else {
 392                 if (data + 6 >= end)
 393                         goto error;
 394
 395                 str = blobmsg_alloc_string_buffer(&status, "timestamp", 32);
 396
 397                 /* year */
 398                 *(str++) = '2';
 399                 *(str++) = '0';
 400                 str = pdu_add_semioctet(str, data[0]);
 401                 /* month */
 402                 *(str++) = '-';
 403                 str = pdu_add_semioctet(str, data[1]);
 404                 /* day */
 405                 *(str++) = '-';
 406                 str = pdu_add_semioctet(str, data[2]);
 407
 408                 /* hour */
 409                 *(str++) = ' ';
 410                 str = pdu_add_semioctet(str, data[3]);
 411                 /* minute */
 412                 *(str++) = ':';
 413                 str = pdu_add_semioctet(str, data[4]);
 414                 /* second */
 415                 *(str++) = ':';
 416                 str = pdu_add_semioctet(str, data[5]);
 417                 *str = 0;
 418
 419                 blobmsg_add_string_buffer(&status);
 420
 421                 data += 7;
 422         }
 423
 424         int message_len = *(data++);
 425         int udh_len = 0;
 426         int bit_offset = 0;
 427
 428         /* User Data Header */
 429         if (first & 0x40) {
 430                 udh_len = decode_udh(data);
 431                 data += udh_len;
 432                 bit_offset = udh_len % 7;
 433                 }
 434
 435         if (data >= end)
 436                 goto error;
 437
 438         switch(dcs & 0x0c) {
 439                 case 0x00:
 440                         /* 7 bit GSM alphabet */
 441                         message_len = message_len - CEILDIV(udh_len * 8, 7);
 442                         message_len = MIN(message_len, CEILDIV((end - data) * 8, 7));
 443                         decode_7bit_field("text", data, message_len, bit_offset);
 444                         break;
 445                 case 0x04:
 446                         /* 8 bit data */
 447                         message_len = MIN(message_len - udh_len, end - data);
 448                         blobmsg_add_hex(&status, "data", data, message_len);
 449                         break;
 450                 case 0x08:
 451                         /* 16 bit UCS-2 string */
 452                         message_len = MIN(message_len - udh_len, end - data);
 453                         blobmsg_add_hex(&status, "ucs-2", data, message_len);
 454                         break;
 455                 default:
 456                         goto error;
 457                 }
 458
 459         blobmsg_close_table(&status, c);
 460         return;
 461
 462 error:
 463         blobmsg_close_table(&status, c);
 464         fprintf(stderr, "There was an error reading message.\n");
 465 }
 466
 467 static enum qmi_cmd_result
 468 cmd_wms_get_message_prepare(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, char *arg)
 469 {
 470         char *err;
 471         int id;
 472
 473         id = strtoul(arg, &err, 10);
 474         if (err && *err) {
 475                 uqmi_add_error("Invalid message ID");
 476                 return QMI_CMD_EXIT;
 477         }
 478
 479         gmreq.data.message_memory_storage_id.memory_index = id;
 480         qmi_set_wms_raw_read_request(msg, &gmreq);
 481
 482         return QMI_CMD_REQUEST;
 483 }
 484
 485
 486 static void cmd_wms_get_raw_message_cb(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg)
 487 {
 488         struct qmi_wms_raw_read_response res;
 489         unsigned char *data;
 490         char *str;
 491         int i;
 492
 493         qmi_parse_wms_raw_read_response(msg, &res);
 494         data = (unsigned char *) res.data.raw_message_data.raw_data;
 495         str = blobmsg_alloc_string_buffer(&status, NULL, res.data.raw_message_data.raw_data_n * 3);
 496         for (i = 0; i < res.data.raw_message_data.raw_data_n; i++) {
 497                 str += sprintf(str, &" %02x"[i ? 0 : 1], data[i]);
 498         }
 499         blobmsg_add_string_buffer(&status);
 500 }
 501
 502 #define cmd_wms_get_raw_message_prepare cmd_wms_get_message_prepare
 503
 504
 505 static struct {
 506         const char *smsc;
 507         const char *target;
 508         bool flash;
 509 } _send;
 510
 511
 512 #define cmd_wms_send_message_smsc_cb no_cb
 513 static enum qmi_cmd_result
 514 cmd_wms_send_message_smsc_prepare(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, char *arg)
 515 {
 516         _send.smsc = arg;
 517         return QMI_CMD_DONE;
 518 }
 519
 520 #define cmd_wms_send_message_target_cb no_cb
 521 static enum qmi_cmd_result
 522 cmd_wms_send_message_target_prepare(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, char *arg)
 523 {
 524         _send.target = arg;
 525         return QMI_CMD_DONE;
 526 }
 527
 528 #define cmd_wms_send_message_flash_cb no_cb
 529 static enum qmi_cmd_result
 530 cmd_wms_send_message_flash_prepare(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, char *arg)
 531 {
 532         _send.flash = true;
 533         return QMI_CMD_DONE;
 534 }
 535
 536 static int
 537 pdu_encode_semioctet(unsigned char *dest, const char *str)
 538 {
 539         int len = 0;
 540         bool lower = true;
 541
 542         while (*str) {
 543                 char digit = *str - '0';
 544
 545                 if (lower)
 546                         dest[len] = 0xf0 | digit;
 547                 else
 548                         dest[len++] &= (digit << 4) | 0xf;
 549
 550                 lower = !lower;
 551                 str++;
 552         }
 553
 554         return lower ? len : (len + 1);
 555 }
 556
 557 static int
 558 pdu_encode_7bit_str(unsigned char *data, const char *str)
 559 {
 560         unsigned char c;
 561         int len = 0;
 562         int ofs = 0;
 563
 564         while(1) {
 565                 c = *(str++) & 0x7f;
 566                 if (!c)
 567                         break;
 568
 569                 switch(ofs) {
 570                 case 0:
 571                         data[len] = c;
 572                         break;
 573                 default:
 574                         data[len++] |= c << (8 - ofs);
 575                         data[len] = c >> ofs;
 576                         break;
 577                 }
 578
 579                 ofs = (ofs + 1) % 8;
 580         }
 581
 582         return len + 1;
 583 }
 584
 585 static int
 586 pdu_encode_number(unsigned char *dest, const char *str, bool smsc)
 587 {
 588         unsigned char format;
 589         bool ascii = false;
 590         int len = 0;
 591         int i;
 592
 593         dest[len++] = 0;
 594         if (*str == '+') {
 595                 str++;
 596                 format = 0x91;
 597         } else {
 598                 format = 0x81;
 599         }
 600
 601         for (i = 0; str[i]; i++) {
 602                 if (str[i] >= '0' && str[i] <= '9')
 603                         continue;
 604
 605                 ascii = true;
 606                 break;
 607         }
 608
 609         if (ascii)
 610                 format |= 0x40;
 611
 612         dest[len++] = format;
 613         if (!ascii)
 614                 len += pdu_encode_semioctet(&dest[len], str);
 615         else
 616                 len += pdu_encode_7bit_str(&dest[len], str);
 617
 618         if (smsc)
 619                 dest[0] = len - 1;
 620         else
 621                 dest[0] = strlen(str);
 622
 623         return len;
 624 }
 625
 626 static int
 627 pdu_encode_data(unsigned char *dest, const char *str)
 628 {
 629         int len = 0;
 630
 631         dest[len++] = 0;
 632         len += pdu_encode_7bit_str(&dest[len], str);
 633         dest[0] = strlen(str);
 634
 635         return len;
 636 }
 637
 638 #define cmd_wms_send_message_cb no_cb
 639 static enum qmi_cmd_result
 640 cmd_wms_send_message_prepare(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, char *arg)
 641 {
 642         static unsigned char buf[512];
 643         static struct qmi_wms_raw_send_request mreq = {
 644                 QMI_INIT_SEQUENCE(raw_message_data,
 645                         .format = QMI_WMS_MESSAGE_FORMAT_GSM_WCDMA_POINT_TO_POINT,
 646                         .raw_data = buf,
 647                 ),
 648         };
 649         unsigned char *cur = buf;
 650         unsigned char first_octet = 0x11;
 651         unsigned char protocol_id = 0x00;
 652         unsigned char dcs = 0x00;
 653
 654         if (!_send.target || !*_send.target) {
 655                 uqmi_add_error("Missing argument");
 656                 return QMI_CMD_EXIT;
 657         }
 658
 659         if ((_send.smsc && strlen(_send.smsc) > 16) || strlen(_send.target) > 16 || strlen(arg) > 160) {
 660                 uqmi_add_error("Argument too long");
 661                 return QMI_CMD_EXIT;
 662         }
 663
 664         if (_send.flash)
 665                 dcs |= 0x10;
 666
 667         if (!_send.smsc || !*_send.smsc)
 668                 *(cur++) = 0;
 669         else
 670                 cur += pdu_encode_number(cur, _send.smsc, true);
 671
 672         *(cur++) = first_octet;
 673         *(cur++) = 0; /* reference */
 674
 675         cur += pdu_encode_number(cur, _send.target, false);
 676         *(cur++) = protocol_id;
 677         *(cur++) = dcs;
 678
 679         *(cur++) = 0xff; /* validity */
 680         cur += pdu_encode_data(cur, arg);
 681
 682         mreq.data.raw_message_data.raw_data_n = cur - buf;
 683         qmi_set_wms_raw_send_request(msg, &mreq);
 684
 685         return QMI_CMD_REQUEST;
 686 }