AMSS/diagfwd.c

   1 /* Copyright (c) 2008-2011, Code Aurora Forum. All rights reserved.
   2  *
   3  * This program is free software; you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License version 2 and
   5  * only version 2 as published by the Free Software Foundation.
   6  *
   7  * This program is distributed in the hope that it will be useful,
   8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
   9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  10  * GNU General Public License for more details.
  11  *
  12  * You should have received a copy of the GNU General Public License
  13  * along with this program; if not, write to the Free Software
  14  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
  15  * 02110-1301, USA.
  16  *
  17  */
  18
  19 #include <linux/slab.h>
  20 #include <linux/init.h>
  21 #include <linux/module.h>
  22 #include <linux/device.h>
  23 #include <linux/err.h>
  24 #include <linux/platform_device.h>
  25 #include <linux/sched.h>
  26 #include <linux/workqueue.h>
  27 #include <linux/pm_runtime.h>
  28 #include <linux/diagchar.h>
  29 #ifdef CONFIG_DIAG_OVER_USB
  30 #include <mach/usbdiag.h>
  31 #endif
  32 #include <mach/msm_smd.h>
  33 #include <mach/socinfo.h>
  34 #include "diagmem.h"
  35 #include "diagchar.h"
  36 #include "diagfwd.h"
  37 #include "diagchar_hdlc.h"
  38 #ifdef CONFIG_DIAG_SDIO_PIPE
  39 #include "diagfwd_sdio.h"
  40 #endif
  41
  42 MODULE_DESCRIPTION("Diag Char Driver");
  43 MODULE_LICENSE("GPL v2");
  44 MODULE_VERSION("1.0");
  45
  46 //add for factory mode
  47 #define OEM_CMD_CODE    0x0e
  48
  49 #if defined(CONFIG_DIAG_EXT_CMDS_FOR_CG)
  50 char app_sub_cmd_array[] = {
  51         0x07,//read GSM BNO
  52         0x08,//write GSM BNO
  53   #ifdef CONFIG_BT
  54         0x18,//bluetooth test
  55   #endif
  56   #ifdef CONFIG_DEVICE_WLAN
  57         0x30,//WLAN send test
  58         0x31,//WLAN recv test
  59   #endif
  60         0x1a,//read GSM IMEI
  61         0x1b,//write GSM IMEI
  62         0x1c,//read GSM SW version
  63         0xc9, //recovery
  64         0x00//get AP and modem version
  65 };
  66 #elif defined(CONFIG_DIAG_EXT_CMDS_FOR_EVDO_ONLY)
  67 char app_sub_cmd_array[] = {
  68   #ifdef CONFIG_BT
  69         0x18,//bluetooth test
  70   #endif
  71   #ifdef CONFIG_DEVICE_WLAN
  72         0x30,//WLAN send test
  73         0x31,//WLAN recv test
  74   #endif
  75         0xc9,//recovery
  76         0x00,//get AP and modem version
  77         0x17 //check camera ID
  78 };
  79 #else
  80 char app_sub_cmd_array[] = {
  81 };
  82 #endif
  83
  84 static int is_cmd_send_to_modem(char cmd_code, char sub_cmd)
  85 {
  86         int i=0;
  87
  88         if(cmd_code != OEM_CMD_CODE)
  89                 return 0;
  90
  91         for(i=0; i<sizeof(app_sub_cmd_array); i++) {
  92                 if(sub_cmd == app_sub_cmd_array[i])
  93                 {
  94                         printk("sub_cmd %d is sent to app\n", sub_cmd);
  95                         return 0;
  96                 }
  97         }
  98         printk("sub_cmd %d is sent to modem\n", sub_cmd);
  99         return 1;
 100 }
 101 //end.
 102
 103 int diag_debug_buf_idx;
 104 unsigned char diag_debug_buf[1024];
 105 static unsigned int buf_tbl_size = 8; /*Number of entries in table of buffers */
 106
 107 struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
 108 struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
 109
 110 #define ENCODE_RSP_AND_SEND(buf_length)                         \
 111 do {                                                                    \
 112         send.state = DIAG_STATE_START;                                  \
 113         send.pkt = driver->apps_rsp_buf;                                \
 114         send.last = (void *)(driver->apps_rsp_buf + buf_length);        \
 115         send.terminate = 1;                                             \
 116         if (!driver->in_busy_1) {                                       \
 117                 enc.dest = driver->buf_in_1;                            \
 118                 enc.dest_last = (void *)(driver->buf_in_1 + 499);       \
 119                 diag_hdlc_encode(&send, &enc);                          \
 120                 driver->write_ptr_1->buf = driver->buf_in_1;            \
 121                 driver->write_ptr_1->length = buf_length + 4;           \
 122                 usb_diag_write(driver->legacy_ch, driver->write_ptr_1); \
 123         }                                                               \
 124 } while (0)
 125
 126 #define CHK_OVERFLOW(bufStart, start, end, length) \
 127 ((bufStart <= start) && (end - start >= length)) ? 1 : 0
 128
 129 #define CHK_APQ_GET_ID() \
 130 (socinfo_get_id() == 86) ? 4062 : 0
 131
 132 void __diag_smd_send_req(void)
 133 {
 134         void *buf = NULL;
 135         int *in_busy_ptr = NULL;
 136         struct diag_request *write_ptr_modem = NULL;
 137
 138         if (!driver->in_busy_1) {
 139                 buf = driver->buf_in_1;
 140                 write_ptr_modem = driver->write_ptr_1;
 141                 in_busy_ptr = &(driver->in_busy_1);
 142         } else if (!driver->in_busy_2) {
 143                 buf = driver->buf_in_2;
 144                 write_ptr_modem = driver->write_ptr_2;
 145                 in_busy_ptr = &(driver->in_busy_2);
 146         }
 147
 148         if (driver->ch && buf) {
 149                 int r = smd_read_avail(driver->ch);
 150
 151                 if (r > IN_BUF_SIZE) {
 152                         if (r < MAX_IN_BUF_SIZE) {
 153                                 printk(KERN_ALERT "\n diag: SMD sending in "
 154                                                    "packets upto %d bytes", r);
 155                                 buf = krealloc(buf, r, GFP_KERNEL);
 156                         } else {
 157                                 printk(KERN_ALERT "\n diag: SMD sending in "
 158                                 "packets more than %d bytes", MAX_IN_BUF_SIZE);
 159                                 return;
 160                         }
 161                 }
 162                 if (r > 0) {
 163                         if (!buf)
 164                                 pr_info("Out of diagmem for Modem\n");
 165                         else {
 166                                 APPEND_DEBUG('i');
 167                                 smd_read(driver->ch, buf, r);
 168                                 APPEND_DEBUG('j');
 169                                 write_ptr_modem->length = r;
 170                                 *in_busy_ptr = 1;
 171                                 diag_device_write(buf, MODEM_DATA,
 172                                                          write_ptr_modem);
 173                         }
 174                 }
 175         }
 176 }
 177
 178 int diag_device_write(void *buf, int proc_num, struct diag_request *write_ptr)
 179 {
 180         int i, err = 0;
 181
 182         if (driver->logging_mode == MEMORY_DEVICE_MODE) {
 183                 if (proc_num == APPS_DATA) {
 184                         for (i = 0; i < driver->poolsize_write_struct; i++)
 185                                 if (driver->buf_tbl[i].length == 0) {
 186                                         driver->buf_tbl[i].buf = buf;
 187                                         driver->buf_tbl[i].length =
 188                                                                  driver->used;
 189 #ifdef DIAG_DEBUG
 190                                         printk(KERN_INFO "\n ENQUEUE buf ptr"
 191                                                    " and length is %x , %d\n",
 192                                                    (unsigned int)(driver->buf_
 193                                 tbl[i].buf), driver->buf_tbl[i].length);
 194 #endif
 195                                         break;
 196                                 }
 197                 }
 198                 for (i = 0; i < driver->num_clients; i++)
 199                         if (driver->client_map[i].pid ==
 200                                                  driver->logging_process_id)
 201                                 break;
 202                 if (i < driver->num_clients) {
 203                         driver->data_ready[i] |= MEMORY_DEVICE_LOG_TYPE;
 204                         wake_up_interruptible(&driver->wait_q);
 205                 } else
 206                         return -EINVAL;
 207         } else if (driver->logging_mode == NO_LOGGING_MODE) {
 208                 if (proc_num == MODEM_DATA) {
 209                         driver->in_busy_1 = 0;
 210                         driver->in_busy_2 = 0;
 211                         queue_work(driver->diag_wq, &(driver->
 212                                                         diag_read_smd_work));
 213                 } else if (proc_num == QDSP_DATA) {
 214                         driver->in_busy_qdsp_1 = 0;
 215                         driver->in_busy_qdsp_2 = 0;
 216                         queue_work(driver->diag_wq, &(driver->
 217                                                 diag_read_smd_qdsp_work));
 218                 }
 219                 err = -1;
 220         }
 221 #ifdef CONFIG_DIAG_OVER_USB
 222         else if (driver->logging_mode == USB_MODE) {
 223                 if (proc_num == APPS_DATA) {
 224                         driver->write_ptr_svc = (struct diag_request *)
 225                         (diagmem_alloc(driver, sizeof(struct diag_request),
 226                                  POOL_TYPE_WRITE_STRUCT));
 227                         if (driver->write_ptr_svc) {
 228                                 driver->write_ptr_svc->length = driver->used;
 229                                 driver->write_ptr_svc->buf = buf;
 230                                 err = usb_diag_write(driver->legacy_ch,
 231                                                 driver->write_ptr_svc);
 232                         } else
 233                                 err = -1;
 234                 } else if (proc_num == MODEM_DATA) {
 235                         write_ptr->buf = buf;
 236 #ifdef DIAG_DEBUG
 237                         printk(KERN_INFO "writing data to USB,"
 238                                 "pkt length %d\n", write_ptr->length);
 239                         print_hex_dump(KERN_DEBUG, "Written Packet Data to"
 240                                            " USB: ", 16, 1, DUMP_PREFIX_ADDRESS,
 241                                             buf, write_ptr->length, 1);
 242 #endif /* DIAG DEBUG */
 243                         err = usb_diag_write(driver->legacy_ch, write_ptr);
 244                 } else if (proc_num == QDSP_DATA) {
 245                         write_ptr->buf = buf;
 246                         err = usb_diag_write(driver->legacy_ch, write_ptr);
 247                 }
 248 #ifdef CONFIG_DIAG_SDIO_PIPE
 249                 else if (proc_num == SDIO_DATA) {
 250                         if (machine_is_msm8x60_charm_surf() ||
 251                                         machine_is_msm8x60_charm_ffa()) {
 252                                 write_ptr->buf = buf;
 253                                 err = usb_diag_write(driver->mdm_ch, write_ptr);
 254                         } else
 255                                 pr_err("diag: Incorrect data while USB write");
 256                 }
 257 #endif
 258                 APPEND_DEBUG('d');
 259         }
 260 #endif /* DIAG OVER USB */
 261     return err;
 262 }
 263
 264 void __diag_smd_qdsp_send_req(void)
 265 {
 266         void *buf = NULL;
 267         int *in_busy_qdsp_ptr = NULL;
 268         struct diag_request *write_ptr_qdsp = NULL;
 269
 270         if (!driver->in_busy_qdsp_1) {
 271                 buf = driver->buf_in_qdsp_1;
 272                 write_ptr_qdsp = driver->write_ptr_qdsp_1;
 273                 in_busy_qdsp_ptr = &(driver->in_busy_qdsp_1);
 274         } else if (!driver->in_busy_qdsp_2) {
 275                 buf = driver->buf_in_qdsp_2;
 276                 write_ptr_qdsp = driver->write_ptr_qdsp_2;
 277                 in_busy_qdsp_ptr = &(driver->in_busy_qdsp_2);
 278         }
 279
 280         if (driver->chqdsp && buf) {
 281                 int r = smd_read_avail(driver->chqdsp);
 282
 283                 if (r > IN_BUF_SIZE) {
 284                         if (r < MAX_IN_BUF_SIZE) {
 285                                 printk(KERN_ALERT "\n diag: SMD sending in "
 286                                                    "packets upto %d bytes", r);
 287                                 buf = krealloc(buf, r, GFP_KERNEL);
 288                         } else {
 289                                 printk(KERN_ALERT "\n diag: SMD sending in "
 290                                 "packets more than %d bytes", MAX_IN_BUF_SIZE);
 291                                 return;
 292                         }
 293                 }
 294                 if (r > 0) {
 295                         if (!buf)
 296                                 printk(KERN_INFO "Out of diagmem for QDSP\n");
 297                         else {
 298                                 APPEND_DEBUG('i');
 299                                 smd_read(driver->chqdsp, buf, r);
 300                                 APPEND_DEBUG('j');
 301                                 write_ptr_qdsp->length = r;
 302                                 *in_busy_qdsp_ptr = 1;
 303                                 diag_device_write(buf, QDSP_DATA,
 304                                                          write_ptr_qdsp);
 305                         }
 306                 }
 307         }
 308 }
 309
 310 static void diag_print_mask_table(void)
 311 {
 312 /* Enable this to print mask table when updated */
 313 #ifdef MASK_DEBUG
 314         int first;
 315         int last;
 316         uint8_t *ptr = driver->msg_masks;
 317         int i = 0;
 318
 319         while (*(uint32_t *)(ptr + 4)) {
 320                 first = *(uint32_t *)ptr;
 321                 ptr += 4;
 322                 last = *(uint32_t *)ptr;
 323                 ptr += 4;
 324                 printk(KERN_INFO "SSID %d - %d\n", first, last);
 325                 for (i = 0 ; i <= last - first ; i++)
 326                         printk(KERN_INFO "MASK:%x\n", *((uint32_t *)ptr + i));
 327                 ptr += ((last - first) + 1)*4;
 328
 329         }
 330 #endif
 331 }
 332
 333 static void diag_update_msg_mask(int start, int end , uint8_t *buf)
 334 {
 335         int found = 0;
 336         int first;
 337         int last;
 338         uint8_t *ptr = driver->msg_masks;
 339         uint8_t *ptr_buffer_start = &(*(driver->msg_masks));
 340         uint8_t *ptr_buffer_end = &(*(driver->msg_masks)) + MSG_MASK_SIZE;
 341
 342         mutex_lock(&driver->diagchar_mutex);
 343         /* First SSID can be zero : So check that last is non-zero */
 344
 345         while (*(uint32_t *)(ptr + 4)) {
 346                 first = *(uint32_t *)ptr;
 347                 ptr += 4;
 348                 last = *(uint32_t *)ptr;
 349                 ptr += 4;
 350                 if (start >= first && start <= last) {
 351                         ptr += (start - first)*4;
 352                         if (end <= last)
 353                                 if (CHK_OVERFLOW(ptr_buffer_start, ptr,
 354                                                   ptr_buffer_end,
 355                                                   (((end - start)+1)*4)))
 356                                         memcpy(ptr, buf , ((end - start)+1)*4);
 357                                 else
 358                                         printk(KERN_CRIT "Not enough"
 359                                                          " buffer space for"
 360                                                          " MSG_MASK\n");
 361                         else
 362                                 printk(KERN_INFO "Unable to copy"
 363                                                  " mask change\n");
 364
 365                         found = 1;
 366                         break;
 367                 } else {
 368                         ptr += ((last - first) + 1)*4;
 369                 }
 370         }
 371         /* Entry was not found - add new table */
 372         if (!found) {
 373                 if (CHK_OVERFLOW(ptr_buffer_start, ptr, ptr_buffer_end,
 374                                   8 + ((end - start) + 1)*4)) {
 375                         memcpy(ptr, &(start) , 4);
 376                         ptr += 4;
 377                         memcpy(ptr, &(end), 4);
 378                         ptr += 4;
 379                         memcpy(ptr, buf , ((end - start) + 1)*4);
 380                 } else
 381                         printk(KERN_CRIT " Not enough buffer"
 382                                          " space for MSG_MASK\n");
 383         }
 384         mutex_unlock(&driver->diagchar_mutex);
 385         diag_print_mask_table();
 386
 387 }
 388
 389 static void diag_update_event_mask(uint8_t *buf, int toggle, int num_bits)
 390 {
 391         uint8_t *ptr = driver->event_masks;
 392         uint8_t *temp = buf + 2;
 393
 394         mutex_lock(&driver->diagchar_mutex);
 395         if (!toggle)
 396                 memset(ptr, 0 , EVENT_MASK_SIZE);
 397         else
 398                 if (CHK_OVERFLOW(ptr, ptr,
 399                                  ptr+EVENT_MASK_SIZE,
 400                                   num_bits/8 + 1))
 401                         memcpy(ptr, temp , num_bits/8 + 1);
 402                 else
 403                         printk(KERN_CRIT "Not enough buffer space "
 404                                          "for EVENT_MASK\n");
 405         mutex_unlock(&driver->diagchar_mutex);
 406 }
 407
 408 static void diag_update_log_mask(int equip_id, uint8_t *buf, int num_items)
 409 {
 410         uint8_t *temp = buf;
 411         struct mask_info {
 412                 int equip_id;
 413                 int index;
 414         };
 415         int i = 0;
 416         unsigned char *ptr_data;
 417         int offset = 8*MAX_EQUIP_ID;
 418         struct mask_info *ptr = (struct mask_info *)driver->log_masks;
 419
 420         mutex_lock(&driver->diagchar_mutex);
 421         /* Check if we already know index of this equipment ID */
 422         for (i = 0; i < MAX_EQUIP_ID; i++) {
 423                 if ((ptr->equip_id == equip_id) && (ptr->index != 0)) {
 424                         offset = ptr->index;
 425                         break;
 426                 }
 427                 if ((ptr->equip_id == 0) && (ptr->index == 0)) {
 428                         /*Reached a null entry */
 429                         ptr->equip_id = equip_id;
 430                         ptr->index = driver->log_masks_length;
 431                         offset = driver->log_masks_length;
 432                         driver->log_masks_length += ((num_items+7)/8);
 433                         break;
 434                 }
 435                 ptr++;
 436         }
 437         ptr_data = driver->log_masks + offset;
 438         if (CHK_OVERFLOW(driver->log_masks, ptr_data, driver->log_masks
 439                                          + LOG_MASK_SIZE, (num_items+7)/8))
 440                 memcpy(ptr_data, temp , (num_items+7)/8);
 441         else
 442                 printk(KERN_CRIT " Not enough buffer space for LOG_MASK\n");
 443         mutex_unlock(&driver->diagchar_mutex);
 444 }
 445
 446 static void diag_update_pkt_buffer(unsigned char *buf)
 447 {
 448         unsigned char *ptr = driver->pkt_buf;
 449         unsigned char *temp = buf;
 450
 451         mutex_lock(&driver->diagchar_mutex);
 452         if (CHK_OVERFLOW(ptr, ptr, ptr + PKT_SIZE, driver->pkt_length))
 453                 memcpy(ptr, temp , driver->pkt_length);
 454         else
 455                 printk(KERN_CRIT " Not enough buffer space for PKT_RESP\n");
 456         mutex_unlock(&driver->diagchar_mutex);
 457 }
 458
 459 void diag_update_userspace_clients(unsigned int type)
 460 {
 461         int i;
 462
 463         mutex_lock(&driver->diagchar_mutex);
 464         for (i = 0; i < driver->num_clients; i++)
 465                 if (driver->client_map[i].pid != 0)
 466                         driver->data_ready[i] |= type;
 467         wake_up_interruptible(&driver->wait_q);
 468         mutex_unlock(&driver->diagchar_mutex);
 469 }
 470
 471 void diag_update_sleeping_process(int process_id)
 472 {
 473         int i;
 474
 475         mutex_lock(&driver->diagchar_mutex);
 476         for (i = 0; i < driver->num_clients; i++)
 477                 if (driver->client_map[i].pid == process_id) {
 478                         driver->data_ready[i] |= PKT_TYPE;
 479                         break;
 480                 }
 481         wake_up_interruptible(&driver->wait_q);
 482         mutex_unlock(&driver->diagchar_mutex);
 483 }
 484
 485 static int diag_process_apps_pkt(unsigned char *buf, int len)
 486 {
 487         uint16_t start;
 488         uint16_t end, subsys_cmd_code;
 489         int i, cmd_code, subsys_id;
 490         int packet_type = 1;
 491         unsigned char *temp = buf;
 492
 493         /* event mask */
 494         if ((*buf == 0x60) && (*(++buf) == 0x0)) {
 495                 diag_update_event_mask(buf, 0, 0);
 496                 diag_update_userspace_clients(EVENT_MASKS_TYPE);
 497         }
 498         /* check for set event mask */
 499         else if (*buf == 0x82) {
 500                 buf += 4;
 501                 diag_update_event_mask(buf, 1, *(uint16_t *)buf);
 502                 diag_update_userspace_clients(
 503                 EVENT_MASKS_TYPE);
 504         }
 505         /* log mask */
 506         else if (*buf == 0x73) {
 507                 buf += 4;
 508                 if (*(int *)buf == 3) {
 509                         buf += 4;
 510                         /* Read Equip ID and pass as first param below*/
 511                         diag_update_log_mask(*(int *)buf, buf+8,
 512                                                          *(int *)(buf+4));
 513                         diag_update_userspace_clients(LOG_MASKS_TYPE);
 514                 }
 515         }
 516         /* Check for set message mask  */
 517         else if ((*buf == 0x7d) && (*(++buf) == 0x4)) {
 518                 buf++;
 519                 start = *(uint16_t *)buf;
 520                 buf += 2;
 521                 end = *(uint16_t *)buf;
 522                 buf += 4;
 523                 diag_update_msg_mask((uint32_t)start, (uint32_t)end , buf);
 524                 diag_update_userspace_clients(MSG_MASKS_TYPE);
 525         }
 526         /* Set all run-time masks
 527         if ((*buf == 0x7d) && (*(++buf) == 0x5)) {
 528                 TO DO
 529         } */
 530 #if defined(CONFIG_DIAG_OVER_USB)
 531          /* Check for ID for APQ8060 AND NO MODEM present */
 532         else if (!(driver->ch) && CHK_APQ_GET_ID()) {
 533                 /* Respond to polling for Apps only DIAG */
 534                 if ((*buf == 0x4b) && (*(buf+1) == 0x32) &&
 535                                                          (*(buf+2) == 0x03)) {
 536                         for (i = 0; i < 3; i++)
 537                                 driver->apps_rsp_buf[i] = *(buf+i);
 538                         for (i = 0; i < 13; i++)
 539                                 driver->apps_rsp_buf[i+3] = 0;
 540
 541                         ENCODE_RSP_AND_SEND(15);
 542                         return 0;
 543                 }
 544                 /* respond to 0x0 command */
 545                 else if (*buf == 0x00) {
 546                         for (i = 0; i < 55; i++)
 547                                 driver->apps_rsp_buf[i] = 0;
 548
 549                         ENCODE_RSP_AND_SEND(54);
 550                         return 0;
 551                 }
 552                 /* respond to 0x7c command */
 553                 else if (*buf == 0x7c) {
 554                         driver->apps_rsp_buf[0] = 0x7c;
 555                         for (i = 1; i < 8; i++)
 556                                 driver->apps_rsp_buf[i] = 0;
 557                         /* Tools ID for APQ 8060 */
 558                         *(int *)(driver->apps_rsp_buf + 8) = CHK_APQ_GET_ID();
 559                         *(unsigned char *)(driver->apps_rsp_buf + 12) = '\0';
 560                         *(unsigned char *)(driver->apps_rsp_buf + 13) = '\0';
 561                         ENCODE_RSP_AND_SEND(13);
 562                         return 0;
 563                 }
 564         }
 565 #endif
 566         /* Check for registered clients and forward packet to user-space */
 567         else{
 568                 cmd_code = (int)(*(char *)buf);
 569                 temp++;
 570                 subsys_id = (int)(*(char *)temp);
 571                 temp++;
 572                 subsys_cmd_code = *(uint16_t *)temp;
 573                 temp += 2;
 574
 575                 printk("cmd_code is %x, subsys_id is %x, subsys_cmd_code is %x\n", cmd_code,subsys_id,subsys_cmd_code);
 576
 577                 for (i = 0; i < diag_max_registration; i++) {
 578                         if (driver->table[i].process_id != 0) {
 579                                 if (driver->table[i].cmd_code ==
 580                                      cmd_code && driver->table[i].subsys_id ==
 581                                      subsys_id &&
 582                                     driver->table[i].cmd_code_lo <=
 583                                      subsys_cmd_code &&
 584                                           driver->table[i].cmd_code_hi >=
 585                                      subsys_cmd_code){
 586                                         driver->pkt_length = len;
 587                                         diag_update_pkt_buffer(buf);
 588                                         diag_update_sleeping_process(
 589                                                 driver->table[i].process_id);
 590                                                 return 0;
 591                                     } /* end of if */
 592                                 else if (driver->table[i].cmd_code == 255
 593                                           && cmd_code == 75) {
 594                                         if (driver->table[i].subsys_id ==
 595                                             subsys_id &&
 596                                            driver->table[i].cmd_code_lo <=
 597                                             subsys_cmd_code &&
 598                                              driver->table[i].cmd_code_hi >=
 599                                             subsys_cmd_code){
 600                                                 driver->pkt_length = len;
 601                                                 diag_update_pkt_buffer(buf);
 602                                                 diag_update_sleeping_process(
 603                                                         driver->table[i].
 604                                                         process_id);
 605                                                 return 0;
 606                                         }
 607                                 } /* end of else-if */
 608                                 else if (driver->table[i].cmd_code == 255 &&
 609                                           driver->table[i].subsys_id == 255) {
 610                                         if (driver->table[i].cmd_code_lo <=
 611                                                          cmd_code &&
 612                                                      driver->table[i].
 613                                                     cmd_code_hi >= cmd_code){
 614                                                 //add for factory mode
 615                                                 if(is_cmd_send_to_modem((char)cmd_code, (char)subsys_id)){
 616                                                         return packet_type;
 617                                             }
 618                                                 //end
 619
 620                                                 driver->pkt_length = len;
 621                                                 diag_update_pkt_buffer(buf);
 622                                                 diag_update_sleeping_process
 623                                                         (driver->table[i].
 624                                                          process_id);
 625                                                 return 0;
 626                                         }
 627                                 } /* end of else-if */
 628                         } /* if(driver->table[i].process_id != 0) */
 629                 }  /* for (i = 0; i < diag_max_registration; i++) */
 630         } /* else */
 631                 return packet_type;
 632 }
 633
 634 void diag_process_hdlc(void *data, unsigned len)
 635 {
 636         struct diag_hdlc_decode_type hdlc;
 637         int ret, type = 0;
 638 #ifdef DIAG_DEBUG
 639         int i;
 640         printk(KERN_INFO "\n HDLC decode function, len of data  %d\n", len);
 641 #endif
 642         hdlc.dest_ptr = driver->hdlc_buf;
 643         hdlc.dest_size = USB_MAX_OUT_BUF;
 644         hdlc.src_ptr = data;
 645         hdlc.src_size = len;
 646         hdlc.src_idx = 0;
 647         hdlc.dest_idx = 0;
 648         hdlc.escaping = 0;
 649
 650         ret = diag_hdlc_decode(&hdlc);
 651
 652         if (ret)
 653                 type = diag_process_apps_pkt(driver->hdlc_buf,
 654                                                           hdlc.dest_idx - 3);
 655         else if (driver->debug_flag) {
 656                 printk(KERN_ERR "Packet dropped due to bad HDLC coding/CRC"
 657                                 " errors or partial packet received, packet"
 658                                 " length = %d\n", len);
 659                 print_hex_dump(KERN_DEBUG, "Dropped Packet Data: ", 16, 1,
 660                                            DUMP_PREFIX_ADDRESS, data, len, 1);
 661                 driver->debug_flag = 0;
 662         }
 663         /* implies this packet is NOT meant for apps */
 664         if (!(driver->ch) && type == 1 && CHK_APQ_GET_ID()) {
 665                 if (driver->chqdsp)
 666                         smd_write(driver->chqdsp, driver->hdlc_buf,
 667                                                          hdlc.dest_idx - 3);
 668                 type = 0;
 669         }
 670
 671 #ifdef DIAG_DEBUG
 672         printk(KERN_INFO "\n hdlc.dest_idx = %d", hdlc.dest_idx);
 673         for (i = 0; i < hdlc.dest_idx; i++)
 674                 printk(KERN_DEBUG "\t%x", *(((unsigned char *)
 675                                                         driver->hdlc_buf)+i));
 676 #endif /* DIAG DEBUG */
 677         /* ignore 2 bytes for CRC, one for 7E and send */
 678         if ((driver->ch) && (ret) && (type) && (hdlc.dest_idx > 3)) {
 679                 APPEND_DEBUG('g');
 680                 smd_write(driver->ch, driver->hdlc_buf, hdlc.dest_idx - 3);
 681                 APPEND_DEBUG('h');
 682 #ifdef DIAG_DEBUG
 683                 printk(KERN_INFO "writing data to SMD, pkt length %d\n", len);
 684                 print_hex_dump(KERN_DEBUG, "Written Packet Data to SMD: ", 16,
 685                                1, DUMP_PREFIX_ADDRESS, data, len, 1);
 686 #endif /* DIAG DEBUG */
 687         }
 688
 689 }
 690
 691 #ifdef CONFIG_DIAG_OVER_USB
 692 #define N_LEGACY_WRITE  (driver->poolsize + 5) /* 2+1 for modem ; 2 for q6 */
 693 #define N_LEGACY_READ   1
 694
 695 int diagfwd_connect(void)
 696 {
 697         int err;
 698
 699         printk(KERN_DEBUG "diag: USB connected\n");
 700         err = usb_diag_alloc_req(driver->legacy_ch, N_LEGACY_WRITE,
 701                         N_LEGACY_READ);
 702         if (err)
 703                 printk(KERN_ERR "diag: unable to alloc USB req on legacy ch");
 704
 705         driver->usb_connected = 1;
 706         driver->in_busy_1 = 0;
 707         driver->in_busy_2 = 0;
 708         driver->in_busy_qdsp_1 = 0;
 709         driver->in_busy_qdsp_2 = 0;
 710
 711         /* Poll SMD channels to check for data*/
 712         queue_work(driver->diag_wq, &(driver->diag_read_smd_work));
 713         queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work));
 714         /* Poll USB channel to check for data*/
 715         queue_work(driver->diag_wq, &(driver->diag_read_work));
 716 #ifdef CONFIG_DIAG_SDIO_PIPE
 717         if (machine_is_msm8x60_charm_surf() || machine_is_msm8x60_charm_ffa()) {
 718                 if (driver->mdm_ch && !IS_ERR(driver->mdm_ch))
 719                         diagfwd_connect_sdio();
 720                 else
 721                         printk(KERN_INFO "diag: No USB MDM ch");
 722         }
 723 #endif
 724         return 0;
 725 }
 726
 727 int diagfwd_disconnect(void)
 728 {
 729         printk(KERN_DEBUG "diag: USB disconnected\n");
 730         driver->usb_connected = 0;
 731         driver->in_busy_1 = 1;
 732         driver->in_busy_2 = 1;
 733         driver->in_busy_qdsp_1 = 1;
 734         driver->in_busy_qdsp_2 = 1;
 735         driver->debug_flag = 1;
 736         usb_diag_free_req(driver->legacy_ch);
 737 #ifdef CONFIG_DIAG_SDIO_PIPE
 738         if (machine_is_msm8x60_charm_surf() || machine_is_msm8x60_charm_ffa())
 739                 if (driver->mdm_ch && !IS_ERR(driver->mdm_ch))
 740                         diagfwd_disconnect_sdio();
 741 #endif
 742         /* TBD - notify and flow control SMD */
 743         return 0;
 744 }
 745
 746 int diagfwd_write_complete(struct diag_request *diag_write_ptr)
 747 {
 748         unsigned char *buf = diag_write_ptr->buf;
 749         /*Determine if the write complete is for data from modem/apps/q6 */
 750         /* Need a context variable here instead */
 751         if (buf == (void *)driver->buf_in_1) {
 752                 driver->in_busy_1 = 0;
 753                 APPEND_DEBUG('o');
 754                 queue_work(driver->diag_wq, &(driver->diag_read_smd_work));
 755         } else if (buf == (void *)driver->buf_in_2) {
 756                 driver->in_busy_2 = 0;
 757                 APPEND_DEBUG('O');
 758                 queue_work(driver->diag_wq, &(driver->diag_read_smd_work));
 759         } else if (buf == (void *)driver->buf_in_qdsp_1) {
 760                 driver->in_busy_qdsp_1 = 0;
 761                 APPEND_DEBUG('p');
 762                 queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work));
 763         } else if (buf == (void *)driver->buf_in_qdsp_2) {
 764                 driver->in_busy_qdsp_2 = 0;
 765                 APPEND_DEBUG('P');
 766                 queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work));
 767         }
 768 #ifdef CONFIG_DIAG_SDIO_PIPE
 769         else if (buf == (void *)driver->buf_in_sdio)
 770                 if (machine_is_msm8x60_charm_surf() ||
 771                                          machine_is_msm8x60_charm_ffa())
 772                         diagfwd_write_complete_sdio();
 773                 else
 774                         pr_err("diag: Incorrect buffer pointer while WRITE");
 775 #endif
 776         else {
 777                 diagmem_free(driver, (unsigned char *)buf, POOL_TYPE_HDLC);
 778                 diagmem_free(driver, (unsigned char *)diag_write_ptr,
 779                                                  POOL_TYPE_WRITE_STRUCT);
 780                 APPEND_DEBUG('q');
 781         }
 782         return 0;
 783 }
 784
 785 int diagfwd_read_complete(struct diag_request *diag_read_ptr)
 786 {
 787         int status = diag_read_ptr->status;
 788         unsigned char *buf = diag_read_ptr->buf;
 789
 790         /* Determine if the read complete is for data on legacy/mdm ch */
 791         if (buf == (void *)driver->usb_buf_out) {
 792                 driver->read_len_legacy = diag_read_ptr->actual;
 793                 APPEND_DEBUG('s');
 794 #ifdef DIAG_DEBUG
 795                 printk(KERN_INFO "read data from USB, pkt length %d",
 796                     diag_read_ptr->actual);
 797                 print_hex_dump(KERN_DEBUG, "Read Packet Data from USB: ", 16, 1,
 798                        DUMP_PREFIX_ADDRESS, diag_read_ptr->buf,
 799                        diag_read_ptr->actual, 1);
 800 #endif /* DIAG DEBUG */
 801                 if (driver->logging_mode == USB_MODE) {
 802                         if (status != -ECONNRESET && status != -ESHUTDOWN)
 803                                 queue_work(driver->diag_wq,
 804                                         &(driver->diag_proc_hdlc_work));
 805                         else
 806                                 queue_work(driver->diag_wq,
 807                                                  &(driver->diag_read_work));
 808                 }
 809         }
 810 #ifdef CONFIG_DIAG_SDIO_PIPE
 811         else if (buf == (void *)driver->usb_buf_mdm_out) {
 812                 if (machine_is_msm8x60_charm_surf() ||
 813                                          machine_is_msm8x60_charm_ffa()) {
 814                         driver->read_len_mdm = diag_read_ptr->actual;
 815                         diagfwd_read_complete_sdio();
 816                 } else
 817                         pr_err("diag: Incorrect buffer pointer while READ");
 818         }
 819 #endif
 820         else
 821                 printk(KERN_ERR "diag: Unknown buffer ptr from USB");
 822
 823         return 0;
 824 }
 825
 826 void diag_read_work_fn(struct work_struct *work)
 827 {
 828         APPEND_DEBUG('d');
 829         driver->usb_read_ptr->buf = driver->usb_buf_out;
 830         driver->usb_read_ptr->length = USB_MAX_OUT_BUF;
 831         usb_diag_read(driver->legacy_ch, driver->usb_read_ptr);
 832         APPEND_DEBUG('e');
 833 }
 834
 835 void diag_process_hdlc_fn(struct work_struct *work)
 836 {
 837         APPEND_DEBUG('D');
 838         diag_process_hdlc(driver->usb_buf_out, driver->read_len_legacy);
 839         diag_read_work_fn(work);
 840         APPEND_DEBUG('E');
 841 }
 842
 843 void diag_usb_legacy_notifier(void *priv, unsigned event,
 844                         struct diag_request *d_req)
 845 {
 846         switch (event) {
 847         case USB_DIAG_CONNECT:
 848                 diagfwd_connect();
 849                 break;
 850         case USB_DIAG_DISCONNECT:
 851                 diagfwd_disconnect();
 852                 break;
 853         case USB_DIAG_READ_DONE:
 854                 diagfwd_read_complete(d_req);
 855                 break;
 856         case USB_DIAG_WRITE_DONE:
 857                 diagfwd_write_complete(d_req);
 858                 break;
 859         default:
 860                 printk(KERN_ERR "Unknown event from USB diag\n");
 861                 break;
 862         }
 863 }
 864
 865 #endif /* DIAG OVER USB */
 866
 867 static void diag_smd_notify(void *ctxt, unsigned event)
 868 {
 869         queue_work(driver->diag_wq, &(driver->diag_read_smd_work));
 870 }
 871
 872 #if defined(CONFIG_MSM_N_WAY_SMD)
 873 static void diag_smd_qdsp_notify(void *ctxt, unsigned event)
 874 {
 875         queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work));
 876 }
 877 #endif
 878
 879 static int diag_smd_probe(struct platform_device *pdev)
 880 {
 881         int r = 0;
 882
 883         if (pdev->id == 0)
 884                 r = smd_open("DIAG", &driver->ch, driver, diag_smd_notify);
 885 #if defined(CONFIG_MSM_N_WAY_SMD)
 886         if (pdev->id == 1)
 887                 r = smd_named_open_on_edge("DIAG", SMD_APPS_QDSP
 888                         , &driver->chqdsp, driver, diag_smd_qdsp_notify);
 889 #endif
 890         pm_runtime_set_active(&pdev->dev);
 891         pm_runtime_enable(&pdev->dev);
 892         printk(KERN_INFO "diag opened SMD port ; r = %d\n", r);
 893
 894         return 0;
 895 }
 896
 897 static int diagfwd_runtime_suspend(struct device *dev)
 898 {
 899         dev_dbg(dev, "pm_runtime: suspending...\n");
 900         return 0;
 901 }
 902
 903 static int diagfwd_runtime_resume(struct device *dev)
 904 {
 905         dev_dbg(dev, "pm_runtime: resuming...\n");
 906         return 0;
 907 }
 908
 909 static const struct dev_pm_ops diagfwd_dev_pm_ops = {
 910         .runtime_suspend = diagfwd_runtime_suspend,
 911         .runtime_resume = diagfwd_runtime_resume,
 912 };
 913
 914 static struct platform_driver msm_smd_ch1_driver = {
 915
 916         .probe = diag_smd_probe,
 917         .driver = {
 918                    .name = "DIAG",
 919                    .owner = THIS_MODULE,
 920                    .pm   = &diagfwd_dev_pm_ops,
 921                    },
 922 };
 923
 924 void diagfwd_init(void)
 925 {
 926         diag_debug_buf_idx = 0;
 927         driver->read_len_legacy = 0;
 928         if (driver->buf_in_1 == NULL)
 929                 driver->buf_in_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
 930                 if (driver->buf_in_1 == NULL)
 931                         goto err;
 932         if (driver->buf_in_2 == NULL)
 933                 driver->buf_in_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
 934                 if (driver->buf_in_2 == NULL)
 935                         goto err;
 936         if (driver->buf_in_qdsp_1 == NULL)
 937                 driver->buf_in_qdsp_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
 938                 if (driver->buf_in_qdsp_1 == NULL)
 939                         goto err;
 940         if (driver->buf_in_qdsp_2 == NULL)
 941                 driver->buf_in_qdsp_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
 942                 if (driver->buf_in_qdsp_2 == NULL)
 943                         goto err;
 944         if (driver->usb_buf_out  == NULL &&
 945              (driver->usb_buf_out = kzalloc(USB_MAX_OUT_BUF,
 946                                          GFP_KERNEL)) == NULL)
 947                 goto err;
 948         if (driver->hdlc_buf == NULL
 949             && (driver->hdlc_buf = kzalloc(HDLC_MAX, GFP_KERNEL)) == NULL)
 950                 goto err;
 951         if (driver->msg_masks == NULL
 952             && (driver->msg_masks = kzalloc(MSG_MASK_SIZE,
 953                                              GFP_KERNEL)) == NULL)
 954                 goto err;
 955         if (driver->log_masks == NULL &&
 956             (driver->log_masks = kzalloc(LOG_MASK_SIZE, GFP_KERNEL)) == NULL)
 957                 goto err;
 958         driver->log_masks_length = 8*MAX_EQUIP_ID;
 959         if (driver->event_masks == NULL &&
 960             (driver->event_masks = kzalloc(EVENT_MASK_SIZE,
 961                                             GFP_KERNEL)) == NULL)
 962                 goto err;
 963         if (driver->client_map == NULL &&
 964             (driver->client_map = kzalloc
 965              ((driver->num_clients) * sizeof(struct diag_client_map),
 966                    GFP_KERNEL)) == NULL)
 967                 goto err;
 968         if (driver->buf_tbl == NULL)
 969                         driver->buf_tbl = kzalloc(buf_tbl_size *
 970                           sizeof(struct diag_write_device), GFP_KERNEL);
 971         if (driver->buf_tbl == NULL)
 972                 goto err;
 973         if (driver->data_ready == NULL &&
 974              (driver->data_ready = kzalloc(driver->num_clients * sizeof(struct
 975                                          diag_client_map), GFP_KERNEL)) == NULL)
 976                 goto err;
 977         if (driver->table == NULL &&
 978              (driver->table = kzalloc(diag_max_registration*
 979                       sizeof(struct diag_master_table),
 980                        GFP_KERNEL)) == NULL)
 981                 goto err;
 982         if (driver->write_ptr_1 == NULL)
 983                 driver->write_ptr_1 = kzalloc(
 984                         sizeof(struct diag_request), GFP_KERNEL);
 985                 if (driver->write_ptr_1 == NULL)
 986                         goto err;
 987         if (driver->write_ptr_2 == NULL)
 988                 driver->write_ptr_2 = kzalloc(
 989                         sizeof(struct diag_request), GFP_KERNEL);
 990                 if (driver->write_ptr_2 == NULL)
 991                         goto err;
 992         if (driver->write_ptr_qdsp_1 == NULL)
 993                 driver->write_ptr_qdsp_1 = kzalloc(
 994                         sizeof(struct diag_request), GFP_KERNEL);
 995                 if (driver->write_ptr_qdsp_1 == NULL)
 996                         goto err;
 997         if (driver->write_ptr_qdsp_2 == NULL)
 998                 driver->write_ptr_qdsp_2 = kzalloc(
 999                         sizeof(struct diag_request), GFP_KERNEL);
1000                 if (driver->write_ptr_qdsp_2 == NULL)
1001                         goto err;
1002         if (driver->usb_read_ptr == NULL)
1003                 driver->usb_read_ptr = kzalloc(
1004                         sizeof(struct diag_request), GFP_KERNEL);
1005                 if (driver->usb_read_ptr == NULL)
1006                         goto err;
1007         if (driver->pkt_buf == NULL &&
1008              (driver->pkt_buf = kzalloc(PKT_SIZE,
1009                          GFP_KERNEL)) == NULL)
1010                 goto err;
1011         if (driver->apps_rsp_buf == NULL)
1012                         driver->apps_rsp_buf = kzalloc(150, GFP_KERNEL);
1013                 if (driver->apps_rsp_buf == NULL)
1014                         goto err;
1015         driver->diag_wq = create_singlethread_workqueue("diag_wq");
1016 #ifdef CONFIG_DIAG_OVER_USB
1017         INIT_WORK(&(driver->diag_proc_hdlc_work), diag_process_hdlc_fn);
1018         INIT_WORK(&(driver->diag_read_work), diag_read_work_fn);
1019         driver->legacy_ch = usb_diag_open(DIAG_LEGACY, driver,
1020                         diag_usb_legacy_notifier);
1021         if (IS_ERR(driver->legacy_ch)) {
1022                 printk(KERN_ERR "Unable to open USB diag legacy channel\n");
1023                 goto err;
1024         }
1025 #ifdef CONFIG_DIAG_SDIO_PIPE
1026         if (machine_is_msm8x60_charm_surf() || machine_is_msm8x60_charm_ffa())
1027                 diagfwd_sdio_init();
1028 #endif
1029 #endif
1030         platform_driver_register(&msm_smd_ch1_driver);
1031
1032         return;
1033 err:
1034                 printk(KERN_INFO "\n Could not initialize diag buffers\n");
1035                 kfree(driver->buf_in_1);
1036                 kfree(driver->buf_in_2);
1037                 kfree(driver->buf_in_qdsp_1);
1038                 kfree(driver->buf_in_qdsp_2);
1039                 kfree(driver->usb_buf_out);
1040                 kfree(driver->hdlc_buf);
1041                 kfree(driver->msg_masks);
1042                 kfree(driver->log_masks);
1043                 kfree(driver->event_masks);
1044                 kfree(driver->client_map);
1045                 kfree(driver->buf_tbl);
1046                 kfree(driver->data_ready);
1047                 kfree(driver->table);
1048                 kfree(driver->pkt_buf);
1049                 kfree(driver->write_ptr_1);
1050                 kfree(driver->write_ptr_2);
1051                 kfree(driver->write_ptr_qdsp_1);
1052                 kfree(driver->write_ptr_qdsp_2);
1053                 kfree(driver->usb_read_ptr);
1054                 kfree(driver->apps_rsp_buf);
1055                 if (driver->diag_wq)
1056                         destroy_workqueue(driver->diag_wq);
1057 }
1058
1059 void diagfwd_exit(void)
1060 {
1061         smd_close(driver->ch);
1062         smd_close(driver->chqdsp);
1063         driver->ch = 0;         /*SMD can make this NULL */
1064         driver->chqdsp = 0;
1065 #ifdef CONFIG_DIAG_OVER_USB
1066         if (driver->usb_connected)
1067                 usb_diag_free_req(driver->legacy_ch);
1068 #endif
1069         platform_driver_unregister(&msm_smd_ch1_driver);
1070 #ifdef CONFIG_DIAG_OVER_USB
1071         usb_diag_close(driver->legacy_ch);
1072 #endif
1073
1074         kfree(driver->buf_in_1);
1075         kfree(driver->buf_in_2);
1076         kfree(driver->buf_in_qdsp_1);
1077         kfree(driver->buf_in_qdsp_2);
1078         kfree(driver->usb_buf_out);
1079         kfree(driver->hdlc_buf);
1080         kfree(driver->msg_masks);
1081         kfree(driver->log_masks);
1082         kfree(driver->event_masks);
1083         kfree(driver->client_map);
1084         kfree(driver->buf_tbl);
1085         kfree(driver->data_ready);
1086         kfree(driver->table);
1087         kfree(driver->pkt_buf);
1088         kfree(driver->write_ptr_1);
1089         kfree(driver->write_ptr_2);
1090         kfree(driver->write_ptr_qdsp_1);
1091         kfree(driver->write_ptr_qdsp_2);
1092         kfree(driver->usb_read_ptr);
1093         kfree(driver->apps_rsp_buf);
1094         destroy_workqueue(driver->diag_wq);
1095 }