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