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