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