PageRenderTime 52ms CodeModel.GetById 12ms app.highlight 32ms RepoModel.GetById 1ms app.codeStats 0ms

/drivers/char/diag/diagfwd.c

https://bitbucket.org/cresqo/cm7-p500-kernel
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}