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 = '' + (val & 0xf); 262 if (*str <= '9') 263 str++; 264 265 *str = '' + ((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++) = ''; 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 - ''; 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] >= '' && 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 } 687
This page was automatically generated by LXR 0.3.1. • OpenWrt