drivers/char/diag/diagfwd.c

   1 /* Copyright (c) 2008-2017, The Linux Foundation. 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 #include <linux/slab.h>
  13 #include <linux/init.h>
  14 #include <linux/module.h>
  15 #include <linux/device.h>
  16 #include <linux/err.h>
  17 #include <linux/sched.h>
  18 #include <linux/ratelimit.h>
  19 #include <linux/workqueue.h>
  20 #include <linux/pm_runtime.h>
  21 #include <linux/diagchar.h>
  22 #include <linux/delay.h>
  23 #include <linux/reboot.h>
  24 #include <linux/of.h>
  25 #include <linux/kmemleak.h>
  26 #ifdef CONFIG_DIAG_OVER_USB
  27 #include <linux/usb/usbdiag.h>
  28 #endif
  29 #include <soc/qcom/socinfo.h>
  30 #include <soc/qcom/restart.h>
  31 #include "diagmem.h"
  32 #include "diagchar.h"
  33 #include "diagfwd.h"
  34 #include "diagfwd_peripheral.h"
  35 #include "diagfwd_cntl.h"
  36 #include "diagchar_hdlc.h"
  37 #include "diag_dci.h"
  38 #include "diag_masks.h"
  39 #include "diag_usb.h"
  40 #include "diag_mux.h"
  41 #include "diag_ipc_logging.h"
  42
  43 #define STM_CMD_VERSION_OFFSET  4
  44 #define STM_CMD_MASK_OFFSET     5
  45 #define STM_CMD_DATA_OFFSET     6
  46 #define STM_CMD_NUM_BYTES       7
  47
  48 #define STM_RSP_SUPPORTED_INDEX         7
  49 #define STM_RSP_STATUS_INDEX            8
  50 #define STM_RSP_NUM_BYTES               9
  51
  52 static int timestamp_switch;
  53 module_param(timestamp_switch, int, 0644);
  54
  55 int wrap_enabled;
  56 uint16_t wrap_count;
  57 static struct diag_hdlc_decode_type *hdlc_decode;
  58
  59 #define DIAG_NUM_COMMON_CMD     1
  60 static uint8_t common_cmds[DIAG_NUM_COMMON_CMD] = {
  61         DIAG_CMD_LOG_ON_DMND
  62 };
  63
  64 static uint8_t hdlc_timer_in_progress;
  65
  66 /* Determine if this device uses a device tree */
  67 #ifdef CONFIG_OF
  68 static int has_device_tree(void)
  69 {
  70         struct device_node *node;
  71
  72         node = of_find_node_by_path("/");
  73         if (node) {
  74                 of_node_put(node);
  75                 return 1;
  76         }
  77         return 0;
  78 }
  79 #else
  80 static int has_device_tree(void)
  81 {
  82         return 0;
  83 }
  84 #endif
  85
  86 int chk_config_get_id(void)
  87 {
  88         switch (socinfo_get_msm_cpu()) {
  89         case MSM_CPU_8X60:
  90                 return APQ8060_TOOLS_ID;
  91         case MSM_CPU_8960:
  92         case MSM_CPU_8960AB:
  93                 return AO8960_TOOLS_ID;
  94         case MSM_CPU_8064:
  95         case MSM_CPU_8064AB:
  96         case MSM_CPU_8064AA:
  97                 return APQ8064_TOOLS_ID;
  98         case MSM_CPU_8930:
  99         case MSM_CPU_8930AA:
 100         case MSM_CPU_8930AB:
 101                 return MSM8930_TOOLS_ID;
 102         case MSM_CPU_8974:
 103                 return MSM8974_TOOLS_ID;
 104         case MSM_CPU_8625:
 105                 return MSM8625_TOOLS_ID;
 106         case MSM_CPU_8084:
 107                 return APQ8084_TOOLS_ID;
 108         case MSM_CPU_8916:
 109                 return MSM8916_TOOLS_ID;
 110         case MSM_CPU_8939:
 111                 return MSM8939_TOOLS_ID;
 112         case MSM_CPU_8994:
 113                 return MSM8994_TOOLS_ID;
 114         case MSM_CPU_8226:
 115                 return APQ8026_TOOLS_ID;
 116         case MSM_CPU_8909:
 117                 return MSM8909_TOOLS_ID;
 118         case MSM_CPU_8992:
 119                 return MSM8992_TOOLS_ID;
 120         case MSM_CPU_8996:
 121                 return MSM_8996_TOOLS_ID;
 122         default:
 123                 if (driver->use_device_tree) {
 124                         if (machine_is_msm8974())
 125                                 return MSM8974_TOOLS_ID;
 126                         else if (machine_is_apq8074())
 127                                 return APQ8074_TOOLS_ID;
 128                         else
 129                                 return 0;
 130                 } else {
 131                         return 0;
 132                 }
 133         }
 134 }
 135
 136 /*
 137  * This will return TRUE for targets which support apps only mode and hence SSR.
 138  * This applies to 8960 and newer targets.
 139  */
 140 int chk_apps_only(void)
 141 {
 142         if (driver->use_device_tree)
 143                 return 1;
 144
 145         switch (socinfo_get_msm_cpu()) {
 146         case MSM_CPU_8960:
 147         case MSM_CPU_8960AB:
 148         case MSM_CPU_8064:
 149         case MSM_CPU_8064AB:
 150         case MSM_CPU_8064AA:
 151         case MSM_CPU_8930:
 152         case MSM_CPU_8930AA:
 153         case MSM_CPU_8930AB:
 154         case MSM_CPU_8627:
 155         case MSM_CPU_9615:
 156         case MSM_CPU_8974:
 157                 return 1;
 158         default:
 159                 return 0;
 160         }
 161 }
 162
 163 /*
 164  * This will return TRUE for targets which support apps as master.
 165  * Thus, SW DLOAD and Mode Reset are supported on apps processor.
 166  * This applies to 8960 and newer targets.
 167  */
 168 int chk_apps_master(void)
 169 {
 170         if (driver->use_device_tree)
 171                 return 1;
 172         else
 173                 return 0;
 174 }
 175
 176 int chk_polling_response(void)
 177 {
 178         if (!(driver->polling_reg_flag) && chk_apps_master())
 179                 /*
 180                  * If the apps processor is master and no other processor
 181                  * has registered to respond for polling
 182                  */
 183                 return 1;
 184         else if (!(driver->diagfwd_cntl[PERIPHERAL_MODEM] &&
 185                    driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open) &&
 186                  (driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask))
 187                 /*
 188                  * If the apps processor is not the master and the modem
 189                  * is not up or we did not receive the feature masks from Modem
 190                  */
 191                 return 1;
 192         else
 193                 return 0;
 194 }
 195
 196 /*
 197  * This function should be called if you feel that the logging process may
 198  * need to be woken up. For instance, if the logging mode is MEMORY_DEVICE MODE
 199  * and while trying to read data from data channel there are no buffers
 200  * available to read the data into, then this function should be called to
 201  * determine if the logging process needs to be woken up.
 202  */
 203 void chk_logging_wakeup(void)
 204 {
 205         int i;
 206         int j;
 207         int pid = 0;
 208
 209         for (j = 0; j < NUM_MD_SESSIONS; j++) {
 210                 if (!driver->md_session_map[j])
 211                         continue;
 212                 pid = driver->md_session_map[j]->pid;
 213
 214                 /* Find the index of the logging process */
 215                 for (i = 0; i < driver->num_clients; i++) {
 216                         if (driver->client_map[i].pid != pid)
 217                                 continue;
 218                         if (driver->data_ready[i] & USER_SPACE_DATA_TYPE)
 219                                 continue;
 220                         /*
 221                          * At very high logging rates a race condition can
 222                          * occur where the buffers containing the data from
 223                          * a channel are all in use, but the data_ready flag
 224                          * is cleared. In this case, the buffers never have
 225                          * their data read/logged. Detect and remedy this
 226                          * situation.
 227                          */
 228                         driver->data_ready[i] |= USER_SPACE_DATA_TYPE;
 229                         pr_debug("diag: Force wakeup of logging process\n");
 230                         wake_up_interruptible(&driver->wait_q);
 231                         break;
 232                 }
 233                 /*
 234                  * Diag Memory Device is in normal. Check only for the first
 235                  * index as all the indices point to the same session
 236                  * structure.
 237                  */
 238                 if ((driver->md_session_mask == DIAG_CON_ALL) && (j == 0))
 239                         break;
 240         }
 241 }
 242
 243 static void pack_rsp_and_send(unsigned char *buf, int len,
 244                                 struct diag_md_session_t *info)
 245 {
 246         int err;
 247         int retry_count = 0, i, rsp_ctxt;
 248         uint32_t write_len = 0;
 249         unsigned long flags;
 250         unsigned char *rsp_ptr = driver->encoded_rsp_buf;
 251         struct diag_pkt_frame_t header;
 252
 253         if (!rsp_ptr || !buf)
 254                 return;
 255
 256         if (len > DIAG_MAX_RSP_SIZE || len < 0) {
 257                 pr_err("diag: In %s, invalid len %d, permissible len %d\n",
 258                        __func__, len, DIAG_MAX_RSP_SIZE);
 259                 return;
 260         }
 261
 262         if (info && info->peripheral_mask) {
 263                 if (info->peripheral_mask == DIAG_CON_ALL ||
 264                         (info->peripheral_mask & (1 << APPS_DATA)) ||
 265                         (info->peripheral_mask & (1 << PERIPHERAL_MODEM))) {
 266                         rsp_ctxt = SET_BUF_CTXT(APPS_DATA, TYPE_CMD, 1);
 267                 } else {
 268                         for (i = 0; i <= NUM_PERIPHERALS; i++) {
 269                                 if (info->peripheral_mask & (1 << i))
 270                                         break;
 271                         }
 272                         rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, 1);
 273                 }
 274         } else
 275                 rsp_ctxt = driver->rsp_buf_ctxt;
 276
 277         /*
 278          * Keep trying till we get the buffer back. It should probably
 279          * take one or two iterations. When this loops till UINT_MAX, it
 280          * means we did not get a write complete for the previous
 281          * response.
 282          */
 283         while (retry_count < UINT_MAX) {
 284                 if (!driver->rsp_buf_busy)
 285                         break;
 286                 /*
 287                  * Wait for sometime and try again. The value 10000 was chosen
 288                  * empirically as an optimum value for USB to complete a write
 289                  */
 290                 usleep_range(10000, 10100);
 291                 retry_count++;
 292
 293                 /*
 294                  * There can be a race conditon that clears the data ready flag
 295                  * for responses. Make sure we don't miss previous wakeups for
 296                  * draining responses when we are in Memory Device Mode.
 297                  */
 298                 if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE ||
 299                                 driver->logging_mode == DIAG_MULTI_MODE)
 300                         chk_logging_wakeup();
 301         }
 302         if (driver->rsp_buf_busy) {
 303                 pr_err("diag: unable to get hold of response buffer\n");
 304                 return;
 305         }
 306
 307         driver->rsp_buf_busy = 1;
 308         header.start = CONTROL_CHAR;
 309         header.version = 1;
 310         header.length = len;
 311         memcpy(rsp_ptr, &header, sizeof(header));
 312         write_len += sizeof(header);
 313         memcpy(rsp_ptr + write_len, buf, len);
 314         write_len += len;
 315         *(uint8_t *)(rsp_ptr + write_len) = CONTROL_CHAR;
 316         write_len += sizeof(uint8_t);
 317
 318         err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, write_len, rsp_ctxt);
 319         if (err) {
 320                 pr_err("diag: In %s, unable to write to mux, err: %d\n",
 321                        __func__, err);
 322                 spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
 323                 driver->rsp_buf_busy = 0;
 324                 spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags);
 325         }
 326 }
 327
 328 static void encode_rsp_and_send(unsigned char *buf, int len,
 329                                 struct diag_md_session_t *info)
 330 {
 331         struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
 332         struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
 333         unsigned char *rsp_ptr = driver->encoded_rsp_buf;
 334         int err, i, rsp_ctxt, retry_count = 0;
 335         unsigned long flags;
 336
 337         if (!rsp_ptr || !buf)
 338                 return;
 339
 340         if (len > DIAG_MAX_RSP_SIZE || len < 0) {
 341                 pr_err("diag: In %s, invalid len %d, permissible len %d\n",
 342                        __func__, len, DIAG_MAX_RSP_SIZE);
 343                 return;
 344         }
 345
 346         if (info && info->peripheral_mask) {
 347                 if (info->peripheral_mask == DIAG_CON_ALL ||
 348                         (info->peripheral_mask & (1 << APPS_DATA)) ||
 349                         (info->peripheral_mask & (1 << PERIPHERAL_MODEM))) {
 350                         rsp_ctxt = SET_BUF_CTXT(APPS_DATA, TYPE_CMD, 1);
 351                 } else {
 352                         for (i = 0; i <= NUM_PERIPHERALS; i++) {
 353                                 if (info->peripheral_mask & (1 << i))
 354                                         break;
 355                         }
 356                         rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, 1);
 357                 }
 358         } else
 359                 rsp_ctxt = driver->rsp_buf_ctxt;
 360
 361         /*
 362          * Keep trying till we get the buffer back. It should probably
 363          * take one or two iterations. When this loops till UINT_MAX, it
 364          * means we did not get a write complete for the previous
 365          * response.
 366          */
 367         while (retry_count < UINT_MAX) {
 368                 if (!driver->rsp_buf_busy)
 369                         break;
 370                 /*
 371                  * Wait for sometime and try again. The value 10000 was chosen
 372                  * empirically as an optimum value for USB to complete a write
 373                  */
 374                 usleep_range(10000, 10100);
 375                 retry_count++;
 376
 377                 /*
 378                  * There can be a race conditon that clears the data ready flag
 379                  * for responses. Make sure we don't miss previous wakeups for
 380                  * draining responses when we are in Memory Device Mode.
 381                  */
 382                 if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE ||
 383                                 driver->logging_mode == DIAG_MULTI_MODE)
 384                         chk_logging_wakeup();
 385         }
 386
 387         if (driver->rsp_buf_busy) {
 388                 pr_err("diag: unable to get hold of response buffer\n");
 389                 return;
 390         }
 391
 392         spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
 393         driver->rsp_buf_busy = 1;
 394         spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags);
 395         send.state = DIAG_STATE_START;
 396         send.pkt = buf;
 397         send.last = (void *)(buf + len - 1);
 398         send.terminate = 1;
 399         enc.dest = rsp_ptr;
 400         enc.dest_last = (void *)(rsp_ptr + DIAG_MAX_HDLC_BUF_SIZE - 1);
 401         diag_hdlc_encode(&send, &enc);
 402         driver->encoded_rsp_len = (int)(enc.dest - (void *)rsp_ptr);
 403         err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, driver->encoded_rsp_len,
 404                              rsp_ctxt);
 405         if (err) {
 406                 pr_err("diag: In %s, Unable to write to device, err: %d\n",
 407                         __func__, err);
 408                 spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
 409                 driver->rsp_buf_busy = 0;
 410                 spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags);
 411         }
 412         memset(buf, '\0', DIAG_MAX_RSP_SIZE);
 413 }
 414
 415 void diag_send_rsp(unsigned char *buf, int len, struct diag_md_session_t *info)
 416 {
 417         struct diag_md_session_t *session_info = NULL;
 418         uint8_t hdlc_disabled;
 419
 420         session_info = (info) ? info :
 421                                 diag_md_session_get_peripheral(APPS_DATA);
 422         if (session_info)
 423                 hdlc_disabled = session_info->hdlc_disabled;
 424         else
 425                 hdlc_disabled = driver->hdlc_disabled;
 426
 427         if (hdlc_disabled)
 428                 pack_rsp_and_send(buf, len, session_info);
 429         else
 430                 encode_rsp_and_send(buf, len, session_info);
 431 }
 432
 433 void diag_update_pkt_buffer(unsigned char *buf, uint32_t len, int type)
 434 {
 435         unsigned char *ptr = NULL;
 436         unsigned char *temp = buf;
 437         int *in_busy = NULL;
 438         uint32_t *length = NULL;
 439         uint32_t max_len = 0;
 440
 441         if (!buf || len == 0) {
 442                 pr_err("diag: In %s, Invalid ptr %pK and length %d\n",
 443                        __func__, buf, len);
 444                 return;
 445         }
 446
 447         switch (type) {
 448         case PKT_TYPE:
 449                 ptr = driver->apps_req_buf;
 450                 length = &driver->apps_req_buf_len;
 451                 max_len = DIAG_MAX_REQ_SIZE;
 452                 in_busy = &driver->in_busy_pktdata;
 453                 break;
 454         case DCI_PKT_TYPE:
 455                 ptr = driver->dci_pkt_buf;
 456                 length = &driver->dci_pkt_length;
 457                 max_len = DCI_BUF_SIZE;
 458                 in_busy = &driver->in_busy_dcipktdata;
 459                 break;
 460         default:
 461                 pr_err("diag: Invalid type %d in %s\n", type, __func__);
 462                 return;
 463         }
 464
 465         mutex_lock(&driver->diagchar_mutex);
 466         if (CHK_OVERFLOW(ptr, ptr, ptr + max_len, len)) {
 467                 memcpy(ptr, temp , len);
 468                 *length = len;
 469                 *in_busy = 1;
 470         } else {
 471                 pr_alert("diag: In %s, no space for response packet, len: %d, type: %d\n",
 472                          __func__, len, type);
 473         }
 474         mutex_unlock(&driver->diagchar_mutex);
 475 }
 476
 477 void diag_update_userspace_clients(unsigned int type)
 478 {
 479         int i;
 480
 481         mutex_lock(&driver->diagchar_mutex);
 482         for (i = 0; i < driver->num_clients; i++)
 483                 if (driver->client_map[i].pid != 0)
 484                         driver->data_ready[i] |= type;
 485         wake_up_interruptible(&driver->wait_q);
 486         mutex_unlock(&driver->diagchar_mutex);
 487 }
 488
 489 void diag_update_md_clients(unsigned int type)
 490 {
 491         int i, j;
 492
 493         mutex_lock(&driver->diagchar_mutex);
 494         for (i = 0; i < NUM_MD_SESSIONS; i++) {
 495                 if (driver->md_session_map[i] != NULL)
 496                         for (j = 0; j < driver->num_clients; j++) {
 497                                 if (driver->client_map[j].pid != 0 &&
 498                                         driver->client_map[j].pid ==
 499                                         driver->md_session_map[i]->pid) {
 500                                         driver->data_ready[j] |= type;
 501                                         break;
 502                                 }
 503                         }
 504         }
 505         wake_up_interruptible(&driver->wait_q);
 506         mutex_unlock(&driver->diagchar_mutex);
 507 }
 508 void diag_update_sleeping_process(int process_id, int data_type)
 509 {
 510         int i;
 511
 512         mutex_lock(&driver->diagchar_mutex);
 513         for (i = 0; i < driver->num_clients; i++)
 514                 if (driver->client_map[i].pid == process_id) {
 515                         driver->data_ready[i] |= data_type;
 516                         break;
 517                 }
 518         wake_up_interruptible(&driver->wait_q);
 519         mutex_unlock(&driver->diagchar_mutex);
 520 }
 521
 522 static int diag_send_data(struct diag_cmd_reg_t *entry, unsigned char *buf,
 523                           int len)
 524 {
 525         if (!entry)
 526                 return -EIO;
 527
 528         if (entry->proc == APPS_DATA) {
 529                 diag_update_pkt_buffer(buf, len, PKT_TYPE);
 530                 diag_update_sleeping_process(entry->pid, PKT_TYPE);
 531                 return 0;
 532         }
 533
 534         return diagfwd_write(entry->proc, TYPE_CMD, buf, len);
 535 }
 536
 537 void diag_process_stm_mask(uint8_t cmd, uint8_t data_mask, int data_type)
 538 {
 539         int status = 0;
 540         if (data_type >= PERIPHERAL_MODEM && data_type <= PERIPHERAL_SENSORS) {
 541                 if (driver->feature[data_type].stm_support) {
 542                         status = diag_send_stm_state(data_type, cmd);
 543                         if (status == 0)
 544                                 driver->stm_state[data_type] = cmd;
 545                 }
 546                 driver->stm_state_requested[data_type] = cmd;
 547         } else if (data_type == APPS_DATA) {
 548                 driver->stm_state[data_type] = cmd;
 549                 driver->stm_state_requested[data_type] = cmd;
 550         }
 551 }
 552
 553 int diag_process_stm_cmd(unsigned char *buf, unsigned char *dest_buf)
 554 {
 555         uint8_t version, mask, cmd;
 556         uint8_t rsp_supported = 0;
 557         uint8_t rsp_status = 0;
 558         int i;
 559
 560         if (!buf || !dest_buf) {
 561                 pr_err("diag: Invalid pointers buf: %pK, dest_buf %pK in %s\n",
 562                        buf, dest_buf, __func__);
 563                 return -EIO;
 564         }
 565
 566         version = *(buf + STM_CMD_VERSION_OFFSET);
 567         mask = *(buf + STM_CMD_MASK_OFFSET);
 568         cmd = *(buf + STM_CMD_DATA_OFFSET);
 569
 570         /*
 571          * Check if command is valid. If the command is asking for
 572          * status, then the processor mask field is to be ignored.
 573          */
 574         if ((version != 2) || (cmd > STATUS_STM) ||
 575                 ((cmd != STATUS_STM) && ((mask == 0) || (0 != (mask >> 4))))) {
 576                 /* Command is invalid. Send bad param message response */
 577                 dest_buf[0] = BAD_PARAM_RESPONSE_MESSAGE;
 578                 for (i = 0; i < STM_CMD_NUM_BYTES; i++)
 579                         dest_buf[i+1] = *(buf + i);
 580                 return STM_CMD_NUM_BYTES+1;
 581         } else if (cmd != STATUS_STM) {
 582                 if (mask & DIAG_STM_MODEM)
 583                         diag_process_stm_mask(cmd, DIAG_STM_MODEM,
 584                                               PERIPHERAL_MODEM);
 585
 586                 if (mask & DIAG_STM_LPASS)
 587                         diag_process_stm_mask(cmd, DIAG_STM_LPASS,
 588                                               PERIPHERAL_LPASS);
 589
 590                 if (mask & DIAG_STM_WCNSS)
 591                         diag_process_stm_mask(cmd, DIAG_STM_WCNSS,
 592                                               PERIPHERAL_WCNSS);
 593
 594                 if (mask & DIAG_STM_SENSORS)
 595                         diag_process_stm_mask(cmd, DIAG_STM_SENSORS,
 596                                                 PERIPHERAL_SENSORS);
 597                 if (mask & DIAG_STM_WDSP)
 598                         diag_process_stm_mask(cmd, DIAG_STM_WDSP,
 599                                                 PERIPHERAL_WDSP);
 600
 601                 if (mask & DIAG_STM_APPS)
 602                         diag_process_stm_mask(cmd, DIAG_STM_APPS, APPS_DATA);
 603         }
 604
 605         for (i = 0; i < STM_CMD_NUM_BYTES; i++)
 606                 dest_buf[i] = *(buf + i);
 607
 608         /* Set mask denoting which peripherals support STM */
 609         if (driver->feature[PERIPHERAL_MODEM].stm_support)
 610                 rsp_supported |= DIAG_STM_MODEM;
 611
 612         if (driver->feature[PERIPHERAL_LPASS].stm_support)
 613                 rsp_supported |= DIAG_STM_LPASS;
 614
 615         if (driver->feature[PERIPHERAL_WCNSS].stm_support)
 616                 rsp_supported |= DIAG_STM_WCNSS;
 617
 618         if (driver->feature[PERIPHERAL_SENSORS].stm_support)
 619                 rsp_supported |= DIAG_STM_SENSORS;
 620
 621         if (driver->feature[PERIPHERAL_WDSP].stm_support)
 622                 rsp_supported |= DIAG_STM_WDSP;
 623
 624         rsp_supported |= DIAG_STM_APPS;
 625
 626         /* Set mask denoting STM state/status for each peripheral/APSS */
 627         if (driver->stm_state[PERIPHERAL_MODEM])
 628                 rsp_status |= DIAG_STM_MODEM;
 629
 630         if (driver->stm_state[PERIPHERAL_LPASS])
 631                 rsp_status |= DIAG_STM_LPASS;
 632
 633         if (driver->stm_state[PERIPHERAL_WCNSS])
 634                 rsp_status |= DIAG_STM_WCNSS;
 635
 636         if (driver->stm_state[PERIPHERAL_SENSORS])
 637                 rsp_status |= DIAG_STM_SENSORS;
 638
 639         if (driver->stm_state[PERIPHERAL_WDSP])
 640                 rsp_status |= DIAG_STM_WDSP;
 641
 642         if (driver->stm_state[APPS_DATA])
 643                 rsp_status |= DIAG_STM_APPS;
 644
 645         dest_buf[STM_RSP_SUPPORTED_INDEX] = rsp_supported;
 646         dest_buf[STM_RSP_STATUS_INDEX] = rsp_status;
 647
 648         return STM_RSP_NUM_BYTES;
 649 }
 650
 651 int diag_process_time_sync_query_cmd(unsigned char *src_buf, int src_len,
 652                                       unsigned char *dest_buf, int dest_len)
 653 {
 654         int write_len = 0;
 655         struct diag_cmd_time_sync_query_req_t *req = NULL;
 656         struct diag_cmd_time_sync_query_rsp_t rsp;
 657
 658         if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
 659                 pr_err("diag: Invalid input in %s, src_buf: %pK, src_len: %d, dest_buf: %pK, dest_len: %d",
 660                         __func__, src_buf, src_len, dest_buf, dest_len);
 661                 return -EINVAL;
 662         }
 663
 664         req = (struct diag_cmd_time_sync_query_req_t *)src_buf;
 665         rsp.header.cmd_code = req->header.cmd_code;
 666         rsp.header.subsys_id = req->header.subsys_id;
 667         rsp.header.subsys_cmd_code = req->header.subsys_cmd_code;
 668         rsp.version = req->version;
 669         rsp.time_api = driver->uses_time_api;
 670         memcpy(dest_buf, &rsp, sizeof(rsp));
 671         write_len = sizeof(rsp);
 672         return write_len;
 673 }
 674
 675 int diag_process_time_sync_switch_cmd(unsigned char *src_buf, int src_len,
 676                                       unsigned char *dest_buf, int dest_len)
 677 {
 678         uint8_t peripheral, status = 0;
 679         struct diag_cmd_time_sync_switch_req_t *req = NULL;
 680         struct diag_cmd_time_sync_switch_rsp_t rsp;
 681         struct diag_ctrl_msg_time_sync time_sync_msg;
 682         int msg_size = sizeof(struct diag_ctrl_msg_time_sync);
 683         int err = 0, write_len = 0;
 684
 685         if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
 686                 pr_err("diag: Invalid input in %s, src_buf: %pK, src_len: %d, dest_buf: %pK, dest_len: %d",
 687                         __func__, src_buf, src_len, dest_buf, dest_len);
 688                 return -EINVAL;
 689         }
 690
 691         req = (struct diag_cmd_time_sync_switch_req_t *)src_buf;
 692         rsp.header.cmd_code = req->header.cmd_code;
 693         rsp.header.subsys_id = req->header.subsys_id;
 694         rsp.header.subsys_cmd_code = req->header.subsys_cmd_code;
 695         rsp.version = req->version;
 696         rsp.time_api = req->time_api;
 697         if ((req->version > 1) || (req->time_api > 1) ||
 698                                         (req->persist_time > 0)) {
 699                 dest_buf[0] = BAD_PARAM_RESPONSE_MESSAGE;
 700                 rsp.time_api_status = 0;
 701                 rsp.persist_time_status = PERSIST_TIME_NOT_SUPPORTED;
 702                 memcpy(dest_buf + 1, &rsp, sizeof(rsp));
 703                 write_len = sizeof(rsp) + 1;
 704                 timestamp_switch = 0;
 705                 return write_len;
 706         }
 707
 708         time_sync_msg.ctrl_pkt_id = DIAG_CTRL_MSG_TIME_SYNC_PKT;
 709         time_sync_msg.ctrl_pkt_data_len = 5;
 710         time_sync_msg.version = 1;
 711         time_sync_msg.time_api = req->time_api;
 712
 713         for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
 714                 err = diagfwd_write(peripheral, TYPE_CNTL, &time_sync_msg,
 715                                         msg_size);
 716                 if (err && err != -ENODEV) {
 717                         pr_err("diag: In %s, unable to write to peripheral: %d, type: %d, len: %d, err: %d\n",
 718                                 __func__, peripheral, TYPE_CNTL,
 719                                 msg_size, err);
 720                         status |= (1 << peripheral);
 721                 }
 722         }
 723
 724         driver->time_sync_enabled = 1;
 725         driver->uses_time_api = req->time_api;
 726
 727         switch (req->time_api) {
 728         case 0:
 729                 timestamp_switch = 0;
 730                 break;
 731         case 1:
 732                 timestamp_switch = 1;
 733                 break;
 734         default:
 735                 timestamp_switch = 0;
 736                 break;
 737         }
 738
 739         rsp.time_api_status = status;
 740         rsp.persist_time_status = PERSIST_TIME_NOT_SUPPORTED;
 741         memcpy(dest_buf, &rsp, sizeof(rsp));
 742         write_len = sizeof(rsp);
 743         return write_len;
 744 }
 745
 746 int diag_cmd_log_on_demand(unsigned char *src_buf, int src_len,
 747                            unsigned char *dest_buf, int dest_len)
 748 {
 749         int write_len = 0;
 750         struct diag_log_on_demand_rsp_t header;
 751
 752         if (!driver->diagfwd_cntl[PERIPHERAL_MODEM] ||
 753             !driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open ||
 754             !driver->log_on_demand_support)
 755                 return 0;
 756
 757         if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
 758                 pr_err("diag: Invalid input in %s, src_buf: %pK, src_len: %d, dest_buf: %pK, dest_len: %d",
 759                        __func__, src_buf, src_len, dest_buf, dest_len);
 760                 return -EINVAL;
 761         }
 762
 763         header.cmd_code = DIAG_CMD_LOG_ON_DMND;
 764         header.log_code = *(uint16_t *)(src_buf + 1);
 765         header.status = 1;
 766         memcpy(dest_buf, &header, sizeof(struct diag_log_on_demand_rsp_t));
 767         write_len += sizeof(struct diag_log_on_demand_rsp_t);
 768
 769         return write_len;
 770 }
 771
 772 int diag_cmd_get_mobile_id(unsigned char *src_buf, int src_len,
 773                            unsigned char *dest_buf, int dest_len)
 774 {
 775         int write_len = 0;
 776         struct diag_pkt_header_t *header = NULL;
 777         struct diag_cmd_ext_mobile_rsp_t rsp;
 778
 779         if (!src_buf || src_len != sizeof(*header) || !dest_buf ||
 780             dest_len < sizeof(rsp))
 781                 return -EIO;
 782
 783         header = (struct diag_pkt_header_t *)src_buf;
 784         rsp.header.cmd_code = header->cmd_code;
 785         rsp.header.subsys_id = header->subsys_id;
 786         rsp.header.subsys_cmd_code = header->subsys_cmd_code;
 787         rsp.version = 2;
 788         rsp.padding[0] = 0;
 789         rsp.padding[1] = 0;
 790         rsp.padding[2] = 0;
 791         rsp.family = 0;
 792         rsp.chip_id = (uint32_t)socinfo_get_id();
 793
 794         memcpy(dest_buf, &rsp, sizeof(rsp));
 795         write_len += sizeof(rsp);
 796
 797         return write_len;
 798 }
 799
 800 int diag_check_common_cmd(struct diag_pkt_header_t *header)
 801 {
 802         int i;
 803
 804         if (!header)
 805                 return -EIO;
 806
 807         for (i = 0; i < DIAG_NUM_COMMON_CMD; i++) {
 808                 if (header->cmd_code == common_cmds[i])
 809                         return 1;
 810         }
 811
 812         return 0;
 813 }
 814
 815 static int diag_cmd_chk_stats(unsigned char *src_buf, int src_len,
 816                               unsigned char *dest_buf, int dest_len)
 817 {
 818         int payload = 0;
 819         int write_len = 0;
 820         struct diag_pkt_header_t *header = NULL;
 821         struct diag_cmd_stats_rsp_t rsp;
 822
 823         if (!src_buf || src_len < sizeof(struct diag_pkt_header_t) ||
 824             !dest_buf || dest_len < sizeof(rsp))
 825                 return -EINVAL;
 826
 827         header = (struct diag_pkt_header_t *)src_buf;
 828
 829         if (header->cmd_code != DIAG_CMD_DIAG_SUBSYS ||
 830             header->subsys_id != DIAG_SS_DIAG)
 831                 return -EINVAL;
 832
 833         switch (header->subsys_cmd_code) {
 834         case DIAG_CMD_OP_GET_MSG_ALLOC:
 835                 payload = driver->msg_stats.alloc_count;
 836                 break;
 837         case DIAG_CMD_OP_GET_MSG_DROP:
 838                 payload = driver->msg_stats.drop_count;
 839                 break;
 840         case DIAG_CMD_OP_RESET_MSG_STATS:
 841                 diag_record_stats(DATA_TYPE_F3, PKT_RESET);
 842                 break;
 843         case DIAG_CMD_OP_GET_LOG_ALLOC:
 844                 payload = driver->log_stats.alloc_count;
 845                 break;
 846         case DIAG_CMD_OP_GET_LOG_DROP:
 847                 payload = driver->log_stats.drop_count;
 848                 break;
 849         case DIAG_CMD_OP_RESET_LOG_STATS:
 850                 diag_record_stats(DATA_TYPE_LOG, PKT_RESET);
 851                 break;
 852         case DIAG_CMD_OP_GET_EVENT_ALLOC:
 853                 payload = driver->event_stats.alloc_count;
 854                 break;
 855         case DIAG_CMD_OP_GET_EVENT_DROP:
 856                 payload = driver->event_stats.drop_count;
 857                 break;
 858         case DIAG_CMD_OP_RESET_EVENT_STATS:
 859                 diag_record_stats(DATA_TYPE_EVENT, PKT_RESET);
 860                 break;
 861         default:
 862                 return -EINVAL;
 863         }
 864
 865         memcpy(&rsp.header, header, sizeof(struct diag_pkt_header_t));
 866         rsp.payload = payload;
 867         write_len = sizeof(rsp);
 868         memcpy(dest_buf, &rsp, sizeof(rsp));
 869
 870         return write_len;
 871 }
 872
 873 static int diag_cmd_disable_hdlc(unsigned char *src_buf, int src_len,
 874                                  unsigned char *dest_buf, int dest_len)
 875 {
 876         struct diag_pkt_header_t *header = NULL;
 877         struct diag_cmd_hdlc_disable_rsp_t rsp;
 878         int write_len = 0;
 879
 880         if (!src_buf || src_len < sizeof(*header) ||
 881             !dest_buf || dest_len < sizeof(rsp)) {
 882                 return -EIO;
 883         }
 884
 885         header = (struct diag_pkt_header_t *)src_buf;
 886         if (header->cmd_code != DIAG_CMD_DIAG_SUBSYS ||
 887             header->subsys_id != DIAG_SS_DIAG ||
 888             header->subsys_cmd_code != DIAG_CMD_OP_HDLC_DISABLE) {
 889                 return -EINVAL;
 890         }
 891
 892         memcpy(&rsp.header, header, sizeof(struct diag_pkt_header_t));
 893         rsp.framing_version = 1;
 894         rsp.result = 0;
 895         write_len = sizeof(rsp);
 896         memcpy(dest_buf, &rsp, sizeof(rsp));
 897
 898         return write_len;
 899 }
 900
 901 void diag_send_error_rsp(unsigned char *buf, int len,
 902                         struct diag_md_session_t *info)
 903 {
 904         /* -1 to accomodate the first byte 0x13 */
 905         if (len > (DIAG_MAX_RSP_SIZE - 1)) {
 906                 pr_err("diag: cannot send err rsp, huge length: %d\n", len);
 907                 return;
 908         }
 909
 910         *(uint8_t *)driver->apps_rsp_buf = DIAG_CMD_ERROR;
 911         memcpy((driver->apps_rsp_buf + sizeof(uint8_t)), buf, len);
 912         diag_send_rsp(driver->apps_rsp_buf, len + 1, info);
 913 }
 914
 915 int diag_process_apps_pkt(unsigned char *buf, int len,
 916                         struct diag_md_session_t *info)
 917 {
 918         int i;
 919         int mask_ret;
 920         int write_len = 0;
 921         unsigned char *temp = NULL;
 922         struct diag_cmd_reg_entry_t entry;
 923         struct diag_cmd_reg_entry_t *temp_entry = NULL;
 924         struct diag_cmd_reg_t *reg_item = NULL;
 925
 926         if (!buf)
 927                 return -EIO;
 928
 929         /* Check if the command is a supported mask command */
 930         mask_ret = diag_process_apps_masks(buf, len, info);
 931         if (mask_ret > 0) {
 932                 diag_send_rsp(driver->apps_rsp_buf, mask_ret, info);
 933                 return 0;
 934         }
 935
 936         temp = buf;
 937         entry.cmd_code = (uint16_t)(*(uint8_t *)temp);
 938         temp += sizeof(uint8_t);
 939         entry.subsys_id = (uint16_t)(*(uint8_t *)temp);
 940         temp += sizeof(uint8_t);
 941         entry.cmd_code_hi = (uint16_t)(*(uint16_t *)temp);
 942         entry.cmd_code_lo = (uint16_t)(*(uint16_t *)temp);
 943         temp += sizeof(uint16_t);
 944
 945         pr_debug("diag: In %s, received cmd %02x %02x %02x\n",
 946                  __func__, entry.cmd_code, entry.subsys_id, entry.cmd_code_hi);
 947
 948         if (*buf == DIAG_CMD_LOG_ON_DMND && driver->log_on_demand_support &&
 949             driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask) {
 950                 write_len = diag_cmd_log_on_demand(buf, len,
 951                                                    driver->apps_rsp_buf,
 952                                                    DIAG_MAX_RSP_SIZE);
 953                 if (write_len > 0)
 954                         diag_send_rsp(driver->apps_rsp_buf, write_len, info);
 955                 return 0;
 956         }
 957
 958         mutex_lock(&driver->cmd_reg_mutex);
 959         temp_entry = diag_cmd_search(&entry, ALL_PROC);
 960         if (temp_entry) {
 961                 reg_item = container_of(temp_entry, struct diag_cmd_reg_t,
 962                                                                 entry);
 963                 if (info) {
 964                         if (MD_PERIPHERAL_MASK(reg_item->proc) &
 965                                 info->peripheral_mask)
 966                                 write_len = diag_send_data(reg_item, buf, len);
 967                 } else {
 968                         if (MD_PERIPHERAL_MASK(reg_item->proc) &
 969                                 driver->logging_mask)
 970                                 diag_send_error_rsp(buf, len, info);
 971                         else
 972                                 write_len = diag_send_data(reg_item, buf, len);
 973                 }
 974                 mutex_unlock(&driver->cmd_reg_mutex);
 975                 return write_len;
 976         }
 977         mutex_unlock(&driver->cmd_reg_mutex);
 978
 979 #if defined(CONFIG_DIAG_OVER_USB)
 980         /* Check for the command/respond msg for the maximum packet length */
 981         if ((*buf == 0x4b) && (*(buf+1) == 0x12) &&
 982                 (*(uint16_t *)(buf+2) == 0x0055)) {
 983                 for (i = 0; i < 4; i++)
 984                         *(driver->apps_rsp_buf+i) = *(buf+i);
 985                 *(uint32_t *)(driver->apps_rsp_buf+4) = DIAG_MAX_REQ_SIZE;
 986                 diag_send_rsp(driver->apps_rsp_buf, 8, info);
 987                 return 0;
 988         } else if ((*buf == 0x4b) && (*(buf+1) == 0x12) &&
 989                 (*(uint16_t *)(buf+2) == DIAG_DIAG_STM)) {
 990                 len = diag_process_stm_cmd(buf, driver->apps_rsp_buf);
 991                 if (len > 0) {
 992                         diag_send_rsp(driver->apps_rsp_buf, len, info);
 993                         return 0;
 994                 }
 995                 return len;
 996         }
 997         /* Check for time sync query command */
 998         else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&
 999                 (*(buf+1) == DIAG_SS_DIAG) &&
1000                 (*(uint16_t *)(buf+2) == DIAG_GET_TIME_API)) {
1001                 write_len = diag_process_time_sync_query_cmd(buf, len,
1002                                                         driver->apps_rsp_buf,
1003                                                         DIAG_MAX_RSP_SIZE);
1004                 if (write_len > 0)
1005                         diag_send_rsp(driver->apps_rsp_buf, write_len, info);
1006                 return 0;
1007         }
1008         /* Check for time sync switch command */
1009         else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&
1010                 (*(buf+1) == DIAG_SS_DIAG) &&
1011                 (*(uint16_t *)(buf+2) == DIAG_SET_TIME_API)) {
1012                 write_len = diag_process_time_sync_switch_cmd(buf, len,
1013                                                         driver->apps_rsp_buf,
1014                                                         DIAG_MAX_RSP_SIZE);
1015                 if (write_len > 0)
1016                         diag_send_rsp(driver->apps_rsp_buf, write_len, info);
1017                 return 0;
1018         }
1019         /* Check for download command */
1020         else if ((chk_apps_master()) && (*buf == 0x3A)) {
1021                 /* send response back */
1022                 driver->apps_rsp_buf[0] = *buf;
1023                 diag_send_rsp(driver->apps_rsp_buf, 1, info);
1024                 msleep(5000);
1025                 /* call download API */
1026                 msm_set_restart_mode(RESTART_DLOAD);
1027                 printk(KERN_CRIT "diag: download mode set, Rebooting SoC..\n");
1028                 kernel_restart(NULL);
1029                 /* Not required, represents that command isnt sent to modem */
1030                 return 0;
1031         }
1032         /* Check for polling for Apps only DIAG */
1033         else if ((*buf == 0x4b) && (*(buf+1) == 0x32) &&
1034                 (*(buf+2) == 0x03)) {
1035                 /* If no one has registered for polling */
1036                 if (chk_polling_response()) {
1037                         /* Respond to polling for Apps only DIAG */
1038                         for (i = 0; i < 3; i++)
1039                                 driver->apps_rsp_buf[i] = *(buf+i);
1040                         for (i = 0; i < 13; i++)
1041                                 driver->apps_rsp_buf[i+3] = 0;
1042
1043                         diag_send_rsp(driver->apps_rsp_buf, 16, info);
1044                         return 0;
1045                 }
1046         }
1047         /* Return the Delayed Response Wrap Status */
1048         else if ((*buf == 0x4b) && (*(buf+1) == 0x32) &&
1049                 (*(buf+2) == 0x04) && (*(buf+3) == 0x0)) {
1050                 memcpy(driver->apps_rsp_buf, buf, 4);
1051                 driver->apps_rsp_buf[4] = wrap_enabled;
1052                 diag_send_rsp(driver->apps_rsp_buf, 5, info);
1053                 return 0;
1054         }
1055         /* Wrap the Delayed Rsp ID */
1056         else if ((*buf == 0x4b) && (*(buf+1) == 0x32) &&
1057                 (*(buf+2) == 0x05) && (*(buf+3) == 0x0)) {
1058                 wrap_enabled = true;
1059                 memcpy(driver->apps_rsp_buf, buf, 4);
1060                 driver->apps_rsp_buf[4] = wrap_count;
1061                 diag_send_rsp(driver->apps_rsp_buf, 6, info);
1062                 return 0;
1063         }
1064         /* Mobile ID Rsp */
1065         else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&
1066                 (*(buf+1) == DIAG_SS_PARAMS) &&
1067                 (*(buf+2) == DIAG_EXT_MOBILE_ID) && (*(buf+3) == 0x0))  {
1068                         write_len = diag_cmd_get_mobile_id(buf, len,
1069                                                    driver->apps_rsp_buf,
1070                                                    DIAG_MAX_RSP_SIZE);
1071                 if (write_len > 0) {
1072                         diag_send_rsp(driver->apps_rsp_buf, write_len, info);
1073                         return 0;
1074                 }
1075         }
1076          /*
1077           * If the apps processor is master and no other
1078           * processor has registered for polling command.
1079           * If modem is not up and we have not received feature
1080           * mask update from modem, in that case APPS should
1081           * respond for 0X7C command
1082           */
1083         else if (chk_apps_master() &&
1084                  !(driver->polling_reg_flag) &&
1085                  !(driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open) &&
1086                  !(driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask)) {
1087                 /* respond to 0x0 command */
1088                 if (*buf == 0x00) {
1089                         for (i = 0; i < 55; i++)
1090                                 driver->apps_rsp_buf[i] = 0;
1091
1092                         diag_send_rsp(driver->apps_rsp_buf, 55, info);
1093                         return 0;
1094                 }
1095                 /* respond to 0x7c command */
1096                 else if (*buf == 0x7c) {
1097                         driver->apps_rsp_buf[0] = 0x7c;
1098                         for (i = 1; i < 8; i++)
1099                                 driver->apps_rsp_buf[i] = 0;
1100                         /* Tools ID for APQ 8060 */
1101                         *(int *)(driver->apps_rsp_buf + 8) =
1102                                                          chk_config_get_id();
1103                         *(unsigned char *)(driver->apps_rsp_buf + 12) = '\0';
1104                         *(unsigned char *)(driver->apps_rsp_buf + 13) = '\0';
1105                         diag_send_rsp(driver->apps_rsp_buf, 14, info);
1106                         return 0;
1107                 }
1108         }
1109         write_len = diag_cmd_chk_stats(buf, len, driver->apps_rsp_buf,
1110                                        DIAG_MAX_RSP_SIZE);
1111         if (write_len > 0) {
1112                 diag_send_rsp(driver->apps_rsp_buf, write_len, info);
1113                 return 0;
1114         }
1115         write_len = diag_cmd_disable_hdlc(buf, len, driver->apps_rsp_buf,
1116                                           DIAG_MAX_RSP_SIZE);
1117         if (write_len > 0) {
1118                 /*
1119                  * This mutex lock is necessary since we need to drain all the
1120                  * pending buffers from peripherals which may be HDLC encoded
1121                  * before disabling HDLC encoding on Apps processor.
1122                  */
1123                 mutex_lock(&driver->hdlc_disable_mutex);
1124                 diag_send_rsp(driver->apps_rsp_buf, write_len, info);
1125                 /*
1126                  * Set the value of hdlc_disabled after sending the response to
1127                  * the tools. This is required since the tools is expecting a
1128                  * HDLC encoded reponse for this request.
1129                  */
1130                 pr_debug("diag: In %s, disabling HDLC encoding\n",
1131                        __func__);
1132                 if (info)
1133                         info->hdlc_disabled = 1;
1134                 else
1135                         driver->hdlc_disabled = 1;
1136                 diag_update_md_clients(HDLC_SUPPORT_TYPE);
1137                 mutex_unlock(&driver->hdlc_disable_mutex);
1138                 return 0;
1139         }
1140 #endif
1141
1142         /* We have now come to the end of the function. */
1143         if (chk_apps_only())
1144                 diag_send_error_rsp(buf, len, info);
1145
1146         return 0;
1147 }
1148
1149 void diag_process_hdlc_pkt(void *data, unsigned len,
1150                            struct diag_md_session_t *info)
1151 {
1152         int err = 0;
1153         int ret = 0;
1154
1155         if (len > DIAG_MAX_HDLC_BUF_SIZE) {
1156                 pr_err("diag: In %s, invalid length: %d\n", __func__, len);
1157                 return;
1158         }
1159
1160         mutex_lock(&driver->diag_hdlc_mutex);
1161         pr_debug("diag: In %s, received packet of length: %d, req_buf_len: %d\n",
1162                  __func__, len, driver->hdlc_buf_len);
1163
1164         if (driver->hdlc_buf_len >= DIAG_MAX_REQ_SIZE) {
1165                 pr_err("diag: In %s, request length is more than supported len. Dropping packet.\n",
1166                        __func__);
1167                 goto fail;
1168         }
1169
1170         hdlc_decode->dest_ptr = driver->hdlc_buf + driver->hdlc_buf_len;
1171         hdlc_decode->dest_size = DIAG_MAX_HDLC_BUF_SIZE - driver->hdlc_buf_len;
1172         hdlc_decode->src_ptr = data;
1173         hdlc_decode->src_size = len;
1174         hdlc_decode->src_idx = 0;
1175         hdlc_decode->dest_idx = 0;
1176
1177         ret = diag_hdlc_decode(hdlc_decode);
1178         /*
1179          * driver->hdlc_buf is of size DIAG_MAX_HDLC_BUF_SIZE. But the decoded
1180          * packet should be within DIAG_MAX_REQ_SIZE.
1181          */
1182         if (driver->hdlc_buf_len + hdlc_decode->dest_idx <= DIAG_MAX_REQ_SIZE) {
1183                 driver->hdlc_buf_len += hdlc_decode->dest_idx;
1184         } else {
1185                 pr_err_ratelimited("diag: In %s, Dropping packet. pkt_size: %d, max: %d\n",
1186                                    __func__,
1187                                    driver->hdlc_buf_len + hdlc_decode->dest_idx,
1188                                    DIAG_MAX_REQ_SIZE);
1189                 goto fail;
1190         }
1191
1192         if (ret == HDLC_COMPLETE) {
1193                 err = crc_check(driver->hdlc_buf, driver->hdlc_buf_len);
1194                 if (err) {
1195                         /* CRC check failed. */
1196                         pr_err_ratelimited("diag: In %s, bad CRC. Dropping packet\n",
1197                                            __func__);
1198                         goto fail;
1199                 }
1200                 driver->hdlc_buf_len -= HDLC_FOOTER_LEN;
1201
1202                 if (driver->hdlc_buf_len < 1) {
1203                         pr_err_ratelimited("diag: In %s, message is too short, len: %d, dest len: %d\n",
1204                                            __func__, driver->hdlc_buf_len,
1205                                            hdlc_decode->dest_idx);
1206                         goto fail;
1207                 }
1208
1209                 err = diag_process_apps_pkt(driver->hdlc_buf,
1210                                             driver->hdlc_buf_len, info);
1211                 if (err < 0)
1212                         goto fail;
1213         } else {
1214                 goto end;
1215         }
1216
1217         driver->hdlc_buf_len = 0;
1218         mutex_unlock(&driver->diag_hdlc_mutex);
1219         return;
1220
1221 fail:
1222         /*
1223          * Tools needs to get a response in order to start its
1224          * recovery algorithm. Send an error response if the
1225          * packet is not in expected format.
1226          */
1227         diag_send_error_rsp(driver->hdlc_buf, driver->hdlc_buf_len, info);
1228         driver->hdlc_buf_len = 0;
1229 end:
1230         mutex_unlock(&driver->diag_hdlc_mutex);
1231 }
1232
1233 static int diagfwd_mux_open(int id, int mode)
1234 {
1235         uint8_t i;
1236         unsigned long flags;
1237
1238         switch (mode) {
1239         case DIAG_USB_MODE:
1240                 driver->usb_connected = 1;
1241                 break;
1242         case DIAG_MEMORY_DEVICE_MODE:
1243                 break;
1244         default:
1245                 return -EINVAL;
1246         }
1247
1248         if (driver->rsp_buf_busy) {
1249                 /*
1250                  * When a client switches from callback mode to USB mode
1251                  * explicitly, there can be a situation when the last response
1252                  * is not drained to the user space application. Reset the
1253                  * in_busy flag in this case.
1254                  */
1255                 spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
1256                 driver->rsp_buf_busy = 0;
1257                 spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags);
1258         }
1259         for (i = 0; i < NUM_PERIPHERALS; i++) {
1260                 diagfwd_open(i, TYPE_DATA);
1261                 diagfwd_open(i, TYPE_CMD);
1262         }
1263         queue_work(driver->diag_real_time_wq, &driver->diag_real_time_work);
1264         return 0;
1265 }
1266
1267 static int diagfwd_mux_close(int id, int mode)
1268 {
1269         uint8_t i;
1270
1271         switch (mode) {
1272         case DIAG_USB_MODE:
1273                 driver->usb_connected = 0;
1274                 break;
1275         case DIAG_MEMORY_DEVICE_MODE:
1276                 break;
1277         default:
1278                 return -EINVAL;
1279         }
1280
1281         if ((driver->logging_mode == DIAG_MULTI_MODE &&
1282                 driver->md_session_mode == DIAG_MD_NONE) ||
1283                 (driver->md_session_mode == DIAG_MD_PERIPHERAL)) {
1284                 /*
1285                  * This case indicates that the USB is removed
1286                  * but there is a client running in background
1287                  * with Memory Device mode.
1288                  */
1289         } else {
1290                 /*
1291                  * With sysfs parameter to clear masks set,
1292                  * peripheral masks are cleared on ODL exit and
1293                  * USB disconnection and buffers are not marked busy.
1294                  * This enables read and drop of stale packets.
1295                  *
1296                  * With sysfs parameter to clear masks cleared,
1297                  * masks are not cleared and buffers are to be marked
1298                  * busy to ensure traffic generated by peripheral
1299                  * are not read
1300                  */
1301                 if (!(diag_mask_param())) {
1302                         for (i = 0; i < NUM_PERIPHERALS; i++) {
1303                                 diagfwd_close(i, TYPE_DATA);
1304                                 diagfwd_close(i, TYPE_CMD);
1305                         }
1306                 }
1307                 /* Re enable HDLC encoding */
1308                 pr_debug("diag: In %s, re-enabling HDLC encoding\n",
1309                        __func__);
1310                 mutex_lock(&driver->hdlc_disable_mutex);
1311                 if (driver->md_session_mode == DIAG_MD_NONE)
1312                         driver->hdlc_disabled = 0;
1313                 mutex_unlock(&driver->hdlc_disable_mutex);
1314                 queue_work(driver->diag_wq,
1315                         &(driver->update_user_clients));
1316         }
1317         queue_work(driver->diag_real_time_wq,
1318                    &driver->diag_real_time_work);
1319         return 0;
1320 }
1321
1322 static uint8_t hdlc_reset;
1323
1324 static void hdlc_reset_timer_start(struct diag_md_session_t *info)
1325 {
1326         mutex_lock(&driver->md_session_lock);
1327         if (!hdlc_timer_in_progress) {
1328                 hdlc_timer_in_progress = 1;
1329                 if (info)
1330                         mod_timer(&info->hdlc_reset_timer,
1331                           jiffies + msecs_to_jiffies(200));
1332                 else
1333                         mod_timer(&driver->hdlc_reset_timer,
1334                           jiffies + msecs_to_jiffies(200));
1335         }
1336         mutex_unlock(&driver->md_session_lock);
1337 }
1338
1339 static void hdlc_reset_timer_func(unsigned long data)
1340 {
1341         pr_debug("diag: In %s, re-enabling HDLC encoding\n",
1342                        __func__);
1343         if (hdlc_reset) {
1344                 driver->hdlc_disabled = 0;
1345                 queue_work(driver->diag_wq,
1346                         &(driver->update_user_clients));
1347         }
1348         hdlc_timer_in_progress = 0;
1349 }
1350
1351 void diag_md_hdlc_reset_timer_func(unsigned long pid)
1352 {
1353         struct diag_md_session_t *session_info = NULL;
1354
1355         pr_debug("diag: In %s, re-enabling HDLC encoding\n",
1356                        __func__);
1357         if (hdlc_reset) {
1358                 session_info = diag_md_session_get_pid(pid);
1359                 if (session_info)
1360                         session_info->hdlc_disabled = 0;
1361                 queue_work(driver->diag_wq,
1362                         &(driver->update_md_clients));
1363         }
1364         hdlc_timer_in_progress = 0;
1365 }
1366
1367 static void diag_hdlc_start_recovery(unsigned char *buf, int len,
1368                                      struct diag_md_session_t *info)
1369 {
1370         int i;
1371         static uint32_t bad_byte_counter;
1372         unsigned char *start_ptr = NULL;
1373         struct diag_pkt_frame_t *actual_pkt = NULL;
1374
1375         hdlc_reset = 1;
1376         hdlc_reset_timer_start(info);
1377
1378         actual_pkt = (struct diag_pkt_frame_t *)buf;
1379         for (i = 0; i < len; i++) {
1380                 if (actual_pkt->start == CONTROL_CHAR &&
1381                         actual_pkt->version == 1 &&
1382                         actual_pkt->length < len &&
1383                         (*(uint8_t *)(buf + sizeof(struct diag_pkt_frame_t) +
1384                         actual_pkt->length) == CONTROL_CHAR)) {
1385                                 start_ptr = &buf[i];
1386                                 break;
1387                 }
1388                 bad_byte_counter++;
1389                 if (bad_byte_counter > (DIAG_MAX_REQ_SIZE +
1390                                 sizeof(struct diag_pkt_frame_t) + 1)) {
1391                         bad_byte_counter = 0;
1392                         pr_err("diag: In %s, re-enabling HDLC encoding\n",
1393                                         __func__);
1394                         mutex_lock(&driver->hdlc_disable_mutex);
1395                         if (info)
1396                                 info->hdlc_disabled = 0;
1397                         else
1398                                 driver->hdlc_disabled = 0;
1399                         mutex_unlock(&driver->hdlc_disable_mutex);
1400                         diag_update_md_clients(HDLC_SUPPORT_TYPE);
1401
1402                         return;
1403                 }
1404         }
1405
1406         if (start_ptr) {
1407                 /* Discard any partial packet reads */
1408                 mutex_lock(&driver->hdlc_recovery_mutex);
1409                 driver->incoming_pkt.processing = 0;
1410                 mutex_unlock(&driver->hdlc_recovery_mutex);
1411                 diag_process_non_hdlc_pkt(start_ptr, len - i, info);
1412         }
1413 }
1414
1415 void diag_process_non_hdlc_pkt(unsigned char *buf, int len,
1416                                struct diag_md_session_t *info)
1417 {
1418         int err = 0;
1419         uint16_t pkt_len = 0;
1420         uint32_t read_bytes = 0;
1421         const uint32_t header_len = sizeof(struct diag_pkt_frame_t);
1422         struct diag_pkt_frame_t *actual_pkt = NULL;
1423         unsigned char *data_ptr = NULL;
1424         struct diag_partial_pkt_t *partial_pkt = NULL;
1425
1426         mutex_lock(&driver->hdlc_recovery_mutex);
1427         if (!buf || len <= 0) {
1428                 mutex_unlock(&driver->hdlc_recovery_mutex);
1429                 return;
1430         }
1431         partial_pkt = &driver->incoming_pkt;
1432         if (!partial_pkt->processing) {
1433                 mutex_unlock(&driver->hdlc_recovery_mutex);
1434                 goto start;
1435         }
1436
1437         if (partial_pkt->remaining > len) {
1438                 if ((partial_pkt->read_len + len) > partial_pkt->capacity) {
1439                         pr_err("diag: Invalid length %d, %d received in %s\n",
1440                                partial_pkt->read_len, len, __func__);
1441                         mutex_unlock(&driver->hdlc_recovery_mutex);
1442                         goto end;
1443                 }
1444                 memcpy(partial_pkt->data + partial_pkt->read_len, buf, len);
1445                 read_bytes += len;
1446                 buf += read_bytes;
1447                 partial_pkt->read_len += len;
1448                 partial_pkt->remaining -= len;
1449         } else {
1450                 if ((partial_pkt->read_len + partial_pkt->remaining) >
1451                                                 partial_pkt->capacity) {
1452                         pr_err("diag: Invalid length during partial read %d, %d received in %s\n",
1453                                partial_pkt->read_len,
1454                                partial_pkt->remaining, __func__);
1455                         mutex_unlock(&driver->hdlc_recovery_mutex);
1456                         goto end;
1457                 }
1458                 memcpy(partial_pkt->data + partial_pkt->read_len, buf,
1459                                                 partial_pkt->remaining);
1460                 read_bytes += partial_pkt->remaining;
1461                 buf += read_bytes;
1462                 partial_pkt->read_len += partial_pkt->remaining;
1463                 partial_pkt->remaining = 0;
1464         }
1465
1466         if (partial_pkt->remaining == 0) {
1467                 actual_pkt = (struct diag_pkt_frame_t *)(partial_pkt->data);
1468                 data_ptr = partial_pkt->data + header_len;
1469                 if (*(uint8_t *)(data_ptr + actual_pkt->length) !=
1470                                                 CONTROL_CHAR) {
1471                         mutex_unlock(&driver->hdlc_recovery_mutex);
1472                         diag_hdlc_start_recovery(buf, len, info);
1473                         mutex_lock(&driver->hdlc_recovery_mutex);
1474                 }
1475                 err = diag_process_apps_pkt(data_ptr,
1476                                             actual_pkt->length, info);
1477                 if (err) {
1478                         pr_err("diag: In %s, unable to process incoming data packet, err: %d\n",
1479                                __func__, err);
1480                         mutex_unlock(&driver->hdlc_recovery_mutex);
1481                         goto end;
1482                 }
1483                 partial_pkt->read_len = 0;
1484                 partial_pkt->total_len = 0;
1485                 partial_pkt->processing = 0;
1486                 mutex_unlock(&driver->hdlc_recovery_mutex);
1487                 goto start;
1488         }
1489         mutex_unlock(&driver->hdlc_recovery_mutex);
1490         goto end;
1491
1492 start:
1493         while (read_bytes < len) {
1494                 actual_pkt = (struct diag_pkt_frame_t *)buf;
1495                 pkt_len = actual_pkt->length;
1496
1497                 if (actual_pkt->start != CONTROL_CHAR) {
1498                         diag_hdlc_start_recovery(buf, len, info);
1499                         diag_send_error_rsp(buf, len, info);
1500                         goto end;
1501                 }
1502                 mutex_lock(&driver->hdlc_recovery_mutex);
1503                 if (pkt_len + header_len > partial_pkt->capacity) {
1504                         pr_err("diag: In %s, incoming data is too large for the request buffer %d\n",
1505                                __func__, pkt_len);
1506                         mutex_unlock(&driver->hdlc_recovery_mutex);
1507                         diag_hdlc_start_recovery(buf, len, info);
1508                         break;
1509                 }
1510                 if ((pkt_len + header_len) > (len - read_bytes)) {
1511                         partial_pkt->read_len = len - read_bytes;
1512                         partial_pkt->total_len = pkt_len + header_len;
1513                         partial_pkt->remaining = partial_pkt->total_len -
1514                                                  partial_pkt->read_len;
1515                         partial_pkt->processing = 1;
1516                         memcpy(partial_pkt->data, buf, partial_pkt->read_len);
1517                         mutex_unlock(&driver->hdlc_recovery_mutex);
1518                         break;
1519                 }
1520                 data_ptr = buf + header_len;
1521                 if (*(uint8_t *)(data_ptr + actual_pkt->length) !=
1522                                                 CONTROL_CHAR) {
1523                         mutex_unlock(&driver->hdlc_recovery_mutex);
1524                         diag_hdlc_start_recovery(buf, len, info);
1525                         mutex_lock(&driver->hdlc_recovery_mutex);
1526                 }
1527                 else
1528                         hdlc_reset = 0;
1529                 err = diag_process_apps_pkt(data_ptr,
1530                                             actual_pkt->length, info);
1531                 if (err) {
1532                         mutex_unlock(&driver->hdlc_recovery_mutex);
1533                         break;
1534                 }
1535                 read_bytes += header_len + pkt_len + 1;
1536                 buf += header_len + pkt_len + 1; /* advance to next pkt */
1537                 mutex_unlock(&driver->hdlc_recovery_mutex);
1538         }
1539 end:
1540         return;
1541 }
1542
1543 static int diagfwd_mux_read_done(unsigned char *buf, int len, int ctxt)
1544 {
1545         if (!buf || len <= 0)
1546                 return -EINVAL;
1547
1548         if (!driver->hdlc_disabled)
1549                 diag_process_hdlc_pkt(buf, len, NULL);
1550         else
1551                 diag_process_non_hdlc_pkt(buf, len, NULL);
1552
1553         diag_mux_queue_read(ctxt);
1554         return 0;
1555 }
1556
1557 static int diagfwd_mux_write_done(unsigned char *buf, int len, int buf_ctxt,
1558                                   int ctxt)
1559 {
1560         unsigned long flags;
1561         int peripheral = -1;
1562         int type = -1;
1563         int num = -1;
1564
1565         if (!buf || len < 0)
1566                 return -EINVAL;
1567
1568         peripheral = GET_BUF_PERIPHERAL(buf_ctxt);
1569         type = GET_BUF_TYPE(buf_ctxt);
1570         num = GET_BUF_NUM(buf_ctxt);
1571
1572         switch (type) {
1573         case TYPE_DATA:
1574                 if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) {
1575                         DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
1576                         "Marking buffer as free after write done p: %d, t: %d, buf_num: %d\n",
1577                                 peripheral, type, num);
1578                         diagfwd_write_done(peripheral, type, num);
1579                         diag_ws_on_copy(DIAG_WS_MUX);
1580                 } else if (peripheral == APPS_DATA) {
1581                         diagmem_free(driver, (unsigned char *)buf,
1582                                      POOL_TYPE_HDLC);
1583                         buf = NULL;
1584                 } else {
1585                         pr_err_ratelimited("diag: Invalid peripheral %d in %s, type: %d\n",
1586                                            peripheral, __func__, type);
1587                 }
1588                 break;
1589         case TYPE_CMD:
1590                 if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) {
1591                         diagfwd_write_done(peripheral, type, num);
1592                 }
1593                 if (peripheral == APPS_DATA ||
1594                                 ctxt == DIAG_MEMORY_DEVICE_MODE) {
1595                         spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
1596                         driver->rsp_buf_busy = 0;
1597                         driver->encoded_rsp_len = 0;
1598                         spin_unlock_irqrestore(&driver->rsp_buf_busy_lock,
1599                                                flags);
1600                 }
1601                 break;
1602         default:
1603                 pr_err_ratelimited("diag: Incorrect data type %d, buf_ctxt: %d in %s\n",
1604                                    type, buf_ctxt, __func__);
1605                 break;
1606         }
1607
1608         return 0;
1609 }
1610
1611 static struct diag_mux_ops diagfwd_mux_ops = {
1612         .open = diagfwd_mux_open,
1613         .close = diagfwd_mux_close,
1614         .read_done = diagfwd_mux_read_done,
1615         .write_done = diagfwd_mux_write_done
1616 };
1617
1618 int diagfwd_init(void)
1619 {
1620         int ret;
1621         int i;
1622
1623         wrap_enabled = 0;
1624         wrap_count = 0;
1625         driver->use_device_tree = has_device_tree();
1626         for (i = 0; i < DIAG_NUM_PROC; i++)
1627                 driver->real_time_mode[i] = 1;
1628         driver->supports_separate_cmdrsp = 1;
1629         driver->supports_apps_hdlc_encoding = 1;
1630         driver->supports_apps_header_untagging = 1;
1631         driver->supports_pd_buffering = 1;
1632         for (i = 0; i < NUM_PERIPHERALS; i++)
1633                 driver->peripheral_untag[i] = 0;
1634         mutex_init(&driver->diag_hdlc_mutex);
1635         mutex_init(&driver->diag_cntl_mutex);
1636         mutex_init(&driver->mode_lock);
1637         driver->encoded_rsp_buf = kzalloc(DIAG_MAX_HDLC_BUF_SIZE +
1638                                 APF_DIAG_PADDING, GFP_KERNEL);
1639         if (!driver->encoded_rsp_buf)
1640                 goto err;
1641         kmemleak_not_leak(driver->encoded_rsp_buf);
1642         hdlc_decode = kzalloc(sizeof(struct diag_hdlc_decode_type),
1643                               GFP_KERNEL);
1644         if (!hdlc_decode)
1645                 goto err;
1646         setup_timer(&driver->hdlc_reset_timer, hdlc_reset_timer_func, 0);
1647         kmemleak_not_leak(hdlc_decode);
1648         driver->encoded_rsp_len = 0;
1649         driver->rsp_buf_busy = 0;
1650         spin_lock_init(&driver->rsp_buf_busy_lock);
1651         driver->user_space_data_busy = 0;
1652         driver->hdlc_buf_len = 0;
1653         INIT_LIST_HEAD(&driver->cmd_reg_list);
1654         driver->cmd_reg_count = 0;
1655         mutex_init(&driver->cmd_reg_mutex);
1656
1657         for (i = 0; i < NUM_PERIPHERALS; i++) {
1658                 driver->feature[i].separate_cmd_rsp = 0;
1659                 driver->feature[i].stm_support = DISABLE_STM;
1660                 driver->feature[i].rcvd_feature_mask = 0;
1661                 driver->feature[i].peripheral_buffering = 0;
1662                 driver->feature[i].pd_buffering = 0;
1663                 driver->feature[i].encode_hdlc = 0;
1664                 driver->feature[i].untag_header =
1665                         DISABLE_PKT_HEADER_UNTAGGING;
1666                 driver->feature[i].mask_centralization = 0;
1667                 driver->feature[i].log_on_demand = 0;
1668                 driver->feature[i].sent_feature_mask = 0;
1669         }
1670
1671         for (i = 0; i < NUM_MD_SESSIONS; i++) {
1672                 driver->buffering_mode[i].peripheral = i;
1673                 driver->buffering_mode[i].mode = DIAG_BUFFERING_MODE_STREAMING;
1674                 driver->buffering_mode[i].high_wm_val = DEFAULT_HIGH_WM_VAL;
1675                 driver->buffering_mode[i].low_wm_val = DEFAULT_LOW_WM_VAL;
1676         }
1677
1678         for (i = 0; i < NUM_STM_PROCESSORS; i++) {
1679                 driver->stm_state_requested[i] = DISABLE_STM;
1680                 driver->stm_state[i] = DISABLE_STM;
1681         }
1682
1683         if (driver->hdlc_buf == NULL) {
1684                 driver->hdlc_buf = kzalloc(DIAG_MAX_HDLC_BUF_SIZE, GFP_KERNEL);
1685                 if (!driver->hdlc_buf)
1686                         goto err;
1687                 kmemleak_not_leak(driver->hdlc_buf);
1688         }
1689         if (driver->user_space_data_buf == NULL)
1690                 driver->user_space_data_buf = kzalloc(USER_SPACE_DATA,
1691                                                         GFP_KERNEL);
1692         if (driver->user_space_data_buf == NULL)
1693                 goto err;
1694         kmemleak_not_leak(driver->user_space_data_buf);
1695         if (driver->client_map == NULL &&
1696             (driver->client_map = kzalloc
1697              ((driver->num_clients) * sizeof(struct diag_client_map),
1698                    GFP_KERNEL)) == NULL)
1699                 goto err;
1700         kmemleak_not_leak(driver->client_map);
1701         if (driver->data_ready == NULL &&
1702              (driver->data_ready = kzalloc(driver->num_clients * sizeof(int)
1703                                                         , GFP_KERNEL)) == NULL)
1704                 goto err;
1705         kmemleak_not_leak(driver->data_ready);
1706         if (driver->apps_req_buf == NULL) {
1707                 driver->apps_req_buf = kzalloc(DIAG_MAX_REQ_SIZE, GFP_KERNEL);
1708                 if (!driver->apps_req_buf)
1709                         goto err;
1710                 kmemleak_not_leak(driver->apps_req_buf);
1711         }
1712         if (driver->dci_pkt_buf == NULL) {
1713                 driver->dci_pkt_buf = kzalloc(DCI_BUF_SIZE, GFP_KERNEL);
1714                 if (!driver->dci_pkt_buf)
1715                         goto err;
1716                 kmemleak_not_leak(driver->dci_pkt_buf);
1717         }
1718         if (driver->apps_rsp_buf == NULL) {
1719                 driver->apps_rsp_buf = kzalloc(DIAG_MAX_RSP_SIZE, GFP_KERNEL);
1720                 if (driver->apps_rsp_buf == NULL)
1721                         goto err;
1722                 kmemleak_not_leak(driver->apps_rsp_buf);
1723         }
1724         driver->diag_wq = create_singlethread_workqueue("diag_wq");
1725         if (!driver->diag_wq)
1726                 goto err;
1727         ret = diag_mux_register(DIAG_LOCAL_PROC, DIAG_LOCAL_PROC,
1728                                 &diagfwd_mux_ops);
1729         if (ret) {
1730                 pr_err("diag: Unable to register with USB, err: %d\n", ret);
1731                 goto err;
1732         }
1733
1734         return 0;
1735 err:
1736         pr_err("diag: In %s, couldn't initialize diag\n", __func__);
1737
1738         diag_usb_exit(DIAG_USB_LOCAL);
1739         kfree(driver->encoded_rsp_buf);
1740         kfree(driver->hdlc_buf);
1741         kfree(driver->client_map);
1742         kfree(driver->data_ready);
1743         kfree(driver->apps_req_buf);
1744         kfree(driver->dci_pkt_buf);
1745         kfree(driver->apps_rsp_buf);
1746         kfree(hdlc_decode);
1747         kfree(driver->user_space_data_buf);
1748         if (driver->diag_wq)
1749                 destroy_workqueue(driver->diag_wq);
1750         return -ENOMEM;
1751 }
1752
1753 void diagfwd_exit(void)
1754 {
1755         kfree(driver->encoded_rsp_buf);
1756         kfree(driver->hdlc_buf);
1757         kfree(hdlc_decode);
1758         kfree(driver->client_map);
1759         kfree(driver->data_ready);
1760         kfree(driver->apps_req_buf);
1761         kfree(driver->dci_pkt_buf);
1762         kfree(driver->apps_rsp_buf);
1763         kfree(driver->user_space_data_buf);
1764         destroy_workqueue(driver->diag_wq);
1765 }