From d0d8ff2f5e33bfd840d0644e66982f5c67738058 Mon Sep 17 00:00:00 2001
From: Dumpeti Sathish Kumar <sdumpeti@codeaurora.org>
Date: Mon, 28 Jun 2021 15:58:06 +0530
Subject: [PATCH] ODL support on Open-Source Diag-Router

---
 Makefile                   |  16 +-
 router/diag.c              |   2 +
 router/diag.h              |  24 ++
 router/diag_cntl.c         | 229 +++++++++++++---
 router/diag_cntl.h         |   7 +
 router/dm.c                | 198 ++++++++++++--
 router/dm.h                |   1 +
 router/peripheral-qrtr.c   |  10 +-
 router/peripheral.c        |   2 +-
 router/peripheral.h        |  20 ++
 router/socket.c            |   7 +-
 router/unix.c              |   2 +-
 tools/diag-log_on_device.c | 656 +++++++++++++++++++++++++++++++++++++++++++++
 tools/send_data.c          |  52 +++-
 14 files changed, 1156 insertions(+), 70 deletions(-)
 create mode 100644 tools/diag-log_on_device.c

diff --git a/Makefile b/Makefile
index 3565102..dc22396 100644
--- a/Makefile
+++ b/Makefile
@@ -5,13 +5,14 @@ HAVE_LIBQRTR=1
 
 DIAG := diag-router
 SEND_DATA := send_data
+LOG_ON_DEVICE:= diag-log_on_device
 
-all: $(DIAG) $(SEND_DATA)
+all: $(DIAG) $(SEND_DATA) $(LOG_ON_DEVICE)
 
 CFLAGS ?= -Wall -g -O2
 ifeq ($(HAVE_LIBUDEV),1)
 CFLAGS += -DHAS_LIBUDEV=1
-LDFLAGS += -ludev
+LDFLAGS += -ludev -lpthread
 endif
 ifeq ($(HAVE_LIBQRTR),1)
 CFLAGS += -DHAS_LIBQRTR=1
@@ -55,9 +56,16 @@ SEND_DATA_OBJS := $(SEND_DATA_SRCS:.c=.o)
 $(SEND_DATA): $(SEND_DATA_OBJS)
 	$(CC) -o $@ $^ $(LDFLAGS)
 
-install: $(DIAG) $(SEND_DATA)
+LOG_ON_DEVICE_SRCS  := tools/diag-log_on_device.c
+LOG_ON_DEVICE_OBJS  := $(LOG_ON_DEVICE_SRCS:.c=.o)
+
+$(LOG_ON_DEVICE): $(LOG_ON_DEVICE_OBJS)
+	$(CC) -o $@ $^ $(LDFLAGS)
+
+install: $(DIAG) $(SEND_DATA) $(LOG_ON_DEVICE)
 	install -D -m 755 $(DIAG) $(DESTDIR)$(prefix)/bin/$(DIAG)
 	install -D -m 755 $(SEND_DATA) $(DESTDIR)$(prefix)/bin/$(SEND_DATA)
+	install -D -m 755 $(LOG_ON_DEVICE) $(DESTDIR)$(prefix)/bin/$(LOG_ON_DEVICE)
 
 clean:
-	rm -f $(DIAG) $(OBJS) $(SEND_DATA) $(SEND_DATA_OBJS)
+	rm -f $(DIAG) $(OBJS) $(SEND_DATA) $(SEND_DATA_OBJS) $(LOG_ON_DEVICE) $(LOG_ON_DEVICE_OBJS)
diff --git a/router/diag.c b/router/diag.c
index b5e443d..5978dc1 100644
--- a/router/diag.c
+++ b/router/diag.c
@@ -131,6 +131,8 @@ int main(int argc, char **argv)
 
 	diag_usb_open("/dev/ffs-diag");
 
+	diag_mux_init();
+
 	ret = diag_unix_open();
 	if (ret < 0)
 		errx(1, "failed to create unix socket dm\n");
diff --git a/router/diag.h b/router/diag.h
index da502bf..bf255d1 100644
--- a/router/diag.h
+++ b/router/diag.h
@@ -59,10 +59,16 @@
 #define DIAG_FEATURE_DCI_EXTENDED_HEADER			BIT(14)
 #define DIAG_FEATURE_DIAG_ID					BIT(15)
 #define DIAG_FEATURE_PKT_HEADER_UNTAG				BIT(16)
+#define DIAG_FEATURE_DIAG_ID_FEATURE_MASK			BIT(19)
 
 #define DIAG_CMD_SUBSYS_DISPATCH       75
 #define DIAG_CMD_SUBSYS_DISPATCH_V2	128
 
+void diag_mux_init(void);
+int dm_del(int );
+
+
+
 struct diag_client;
 
 struct peripheral {
@@ -76,6 +82,8 @@ struct peripheral {
 	struct list_head cntlq;
 	struct list_head dataq;
 
+	int periph_id;
+
 	int cntl_fd;
 	bool cntl_open;
 	int data_fd;
@@ -96,6 +104,22 @@ struct peripheral {
 	void (*close)(struct peripheral *perif);
 };
 
+extern struct diag_client *socket_dm;
+
+struct diag_logger_ops {
+	int (*write)(unsigned char *buf, int len, struct watch_flow *flow);
+	int (*open)(void);
+	int (*close)(void);
+	int (*open_device)(int id);
+	int (*close_device)(int id);
+};
+
+struct diag_logger_t {
+	int mode;
+	struct diag_logger_ops *log_ops;
+};
+
+
 extern struct list_head peripherals;
 
 struct diag_cmd {
diff --git a/router/diag_cntl.c b/router/diag_cntl.c
index 0e35c8d..f1a391a 100644
--- a/router/diag_cntl.c
+++ b/router/diag_cntl.c
@@ -43,30 +43,30 @@
 #include "peripheral.h"
 #include "util.h"
 
-#define DIAG_CTRL_MSG_DTR               2
-#define DIAG_CTRL_MSG_DIAGMODE          3
-#define DIAG_CTRL_MSG_DIAGDATA          4
-#define DIAG_CTRL_MSG_FEATURE           8
-#define DIAG_CTRL_MSG_EQUIP_LOG_MASK    9
-#define DIAG_CTRL_MSG_EVENT_MASK_V2     10
-#define DIAG_CTRL_MSG_F3_MASK_V2        11
-#define DIAG_CTRL_MSG_NUM_PRESETS       12
-#define DIAG_CTRL_MSG_SET_PRESET_ID     13
-#define DIAG_CTRL_MSG_LOG_MASK_WITH_PRESET_ID   14
-#define DIAG_CTRL_MSG_EVENT_MASK_WITH_PRESET_ID 15
-#define DIAG_CTRL_MSG_F3_MASK_WITH_PRESET_ID    16
-#define DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE 17
-#define DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM  18
-#define DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL 19
-#define DIAG_CTRL_MSG_DCI_CONNECTION_STATUS     20
-#define DIAG_CTRL_MSG_LAST_EVENT_REPORT         22
-#define DIAG_CTRL_MSG_LOG_RANGE_REPORT          23
-#define DIAG_CTRL_MSG_SSID_RANGE_REPORT         24
-#define DIAG_CTRL_MSG_BUILD_MASK_REPORT         25
-#define DIAG_CTRL_MSG_DEREG             27
-#define DIAG_CTRL_MSG_DCI_HANDSHAKE_PKT         29
-#define DIAG_CTRL_MSG_PD_STATUS                 30
-#define DIAG_CTRL_MSG_TIME_SYNC_PKT             31
+#define DIAG_CTRL_MSG_DTR		2
+#define DIAG_CTRL_MSG_DIAGMODE		3
+#define DIAG_CTRL_MSG_DIAGDATA		4
+#define DIAG_CTRL_MSG_FEATURE		8
+#define DIAG_CTRL_MSG_EQUIP_LOG_MASK	9
+#define DIAG_CTRL_MSG_EVENT_MASK_V2	10
+#define DIAG_CTRL_MSG_F3_MASK_V2	11
+#define DIAG_CTRL_MSG_NUM_PRESETS	12
+#define DIAG_CTRL_MSG_SET_PRESET_ID	13
+#define DIAG_CTRL_MSG_LOG_MASK_WITH_PRESET_ID	14
+#define DIAG_CTRL_MSG_EVENT_MASK_WITH_PRESET_ID	15
+#define DIAG_CTRL_MSG_F3_MASK_WITH_PRESET_ID	16
+#define DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE	17
+#define DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM	18
+#define DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL	19
+#define DIAG_CTRL_MSG_DCI_CONNECTION_STATUS	20
+#define DIAG_CTRL_MSG_LAST_EVENT_REPORT		22
+#define DIAG_CTRL_MSG_LOG_RANGE_REPORT		23
+#define DIAG_CTRL_MSG_SSID_RANGE_REPORT		24
+#define DIAG_CTRL_MSG_BUILD_MASK_REPORT		25
+#define DIAG_CTRL_MSG_DEREG			27
+#define DIAG_CTRL_MSG_DCI_HANDSHAKE_PKT		29
+#define DIAG_CTRL_MSG_PD_STATUS			30
+#define DIAG_CTRL_MSG_TIME_SYNC_PKT		31
 
 struct diag_cntl_hdr {
 	uint32_t cmd;
@@ -190,9 +190,9 @@ struct diag_cntl_cmd_buffering_tx_mode_v2
 	uint8_t tx_mode;
 } __packed;
 
-#define DIAG_BUFFERING_MODE_STREAMING   0
-#define DIAG_BUFFERING_MODE_THRESHOLD   1
-#define DIAG_BUFFERING_MODE_CIRCULAR    2
+#define DIAG_BUFFERING_MODE_STREAMING	0
+#define DIAG_BUFFERING_MODE_THRESHOLD	1
+#define DIAG_BUFFERING_MODE_CIRCULAR	2
 
 #define DIAG_CNTL_CMD_DEREGISTER	27
 struct diag_cntl_cmd_dereg {
@@ -205,10 +205,36 @@ struct diag_cntl_cmd_dereg {
 } __packed;
 #define to_cmd_dereg(h) container_of(h, struct diag_cntl_cmd_dereg, hdr)
 
+#define DIAG_CNTL_CMD_DIAG_ID 33
+
+struct diag_cntl_cmd_diag_id {
+	 struct diag_cntl_hdr hdr;
+	uint32_t version;
+	uint32_t diag_id;
+	char process_name[MAX_DIAGID_STR_LEN];
+} __packed;
+#define to_cmd_diag_id(h) container_of(h, struct diag_cntl_cmd_diag_id, hdr)
+
+struct diag_id_tbl_t {
+	struct list_head link;
+	uint8_t diag_id;
+	uint8_t pd_val;
+	int periph_id;
+	uint8_t pd_feature_mask;
+	char *process_name;
+}; //__packed;
+
+struct diag_id_t {
+	uint8_t diag_id;
+	uint8_t len;
+	char *process_name;
+} __packed;
+
+
 static void diag_cntl_send_feature_mask(struct peripheral *peripheral, uint32_t mask);
 
 static int diag_cntl_register(struct peripheral *peripheral,
-			      struct diag_cntl_hdr *hdr, size_t len)
+				struct diag_cntl_hdr *hdr, size_t len)
 {
 	struct diag_cntl_cmd_reg *pkt = to_cmd_reg(hdr);
 	struct diag_cmd *dc;
@@ -229,7 +255,7 @@ static int diag_cntl_register(struct peripheral *peripheral,
 		last = cmd << 24 | subsys << 16 | pkt->ranges[i].last;
 
 		// printf("[%s] register 0x%x - 0x%x\n",
-		//	  peripheral->name, first, last);
+			//peripheral->name, first, last);
 
 		dc = malloc(sizeof(*dc));
 		if (!dc) {
@@ -249,7 +275,7 @@ static int diag_cntl_register(struct peripheral *peripheral,
 }
 
 static int diag_cntl_feature_mask(struct peripheral *peripheral,
-				  struct diag_cntl_hdr *hdr, size_t len)
+					struct diag_cntl_hdr *hdr, size_t len)
 {
 	struct diag_cntl_cmd_feature *pkt = to_cmd_feature(hdr);
 	uint32_t local_mask = 0;
@@ -263,6 +289,10 @@ static int diag_cntl_feature_mask(struct peripheral *peripheral,
 	if (peripheral->sockets)
 		local_mask |= DIAG_FEATURE_SOCKETS_ENABLED;
 
+	local_mask |= DIAG_FEATURE_DIAG_ID;
+	local_mask |= DIAG_FEATURE_DIAG_ID_FEATURE_MASK;
+
+
 	printf("[%s] mask:", peripheral->name);
 
 	if (mask & DIAG_FEATURE_FEATURE_MASK_SUPPORT)
@@ -288,6 +318,10 @@ static int diag_cntl_feature_mask(struct peripheral *peripheral,
 	if (mask & DIAG_FEATURE_DIAG_ID)
 		printf(" DIAG-ID");
 
+	if (mask & DIAG_FEATURE_DIAG_ID_FEATURE_MASK)
+		printf("DIAG-ID-FEATURE-MASK");
+
+
 	printf(" (0x%x)\n", mask);
 
 	peripheral->features = mask & local_mask;
@@ -395,6 +429,7 @@ void diag_cntl_send_masks(struct peripheral *peripheral)
 
 		diag_cntl_send_msg_mask(peripheral, &range);
 	}
+	diag_cntl_send_event_mask(peripheral);
 }
 
 void diag_cntl_send_event_mask(struct peripheral *peripheral)
@@ -437,7 +472,7 @@ void diag_cntl_send_event_mask(struct peripheral *peripheral)
 }
 
 static int diag_cntl_deregister(struct peripheral *peripheral,
-			      struct diag_cntl_hdr *hdr, size_t len)
+				struct diag_cntl_hdr *hdr, size_t len)
 {
 	struct diag_cntl_cmd_dereg *pkt = to_cmd_dereg(hdr);
 	struct diag_cmd *dc;
@@ -494,6 +529,133 @@ static void diag_cntl_send_feature_mask(struct peripheral *peripheral, uint32_t
 	diag_cntl_set_buffering_mode(peripheral, 0);
 }
 
+#define DIAG_ID_APPS 1
+#define DIAG_ID_ROOT_STRING "root"
+
+struct list_head diag_id_list = LIST_INIT(diag_id_list);
+
+
+int diag_add_diag_id_to_list(uint8_t diag_id, char *process_name,
+				uint8_t pd_val, struct peripheral *perif)
+{
+	struct diag_id_tbl_t *new_item = NULL;
+	int process_len = 0;
+
+	if (!process_name || diag_id == 0)
+		return -EINVAL;
+
+	new_item = (struct diag_id_tbl_t *)malloc(sizeof(struct diag_id_tbl_t));
+	if (!new_item)
+		return -ENOMEM;
+
+	process_len = strlen(process_name);
+	new_item->process_name = malloc(process_len + 1);
+
+	if (!new_item->process_name) {
+		free(new_item);
+		new_item = NULL;
+		return -ENOMEM;
+	}
+	new_item->diag_id = diag_id;
+	new_item->pd_val = pd_val;
+	new_item->periph_id = perif->periph_id;
+	strncpy(new_item->process_name, process_name, process_len + 1);
+	list_add(&diag_id_list, &new_item->link);
+
+	return 0;
+}
+
+static int diag_query_pd_name(char *process_name, char *search_str)
+{
+	if (!process_name)
+		return -EINVAL;
+
+	if (strstr(process_name, search_str))
+		return 1;
+
+	return 0;
+}
+
+
+int diag_query_diag_id(char *process_name, uint8_t *diag_id)
+{
+	struct list_head *start;
+	struct diag_id_tbl_t *item = NULL;
+
+	if (!process_name || !diag_id)
+		return -EINVAL;
+
+	list_for_each(start, &diag_id_list) {
+		item = list_entry(start, struct diag_id_tbl_t, link);
+		if (strcmp(item->process_name, process_name) == 0) {
+			*diag_id = item->diag_id;
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+int diag_query_pd(char *process_name)
+{
+	if (!process_name)
+		return -EINVAL;
+
+	if (diag_query_pd_name(process_name, "modem/root_pd"))
+		return PERIPHERAL_MODEM;
+	if (diag_query_pd_name(process_name, "wpss/root_pd"))
+		return PERIPHERAL_WCNSS;
+	if (diag_query_pd_name(process_name, "wlan_pd"))
+		return UPD_WLAN;
+
+	return PD_UNKNOWN;
+}
+
+
+static int diag_cntl_process_diag_id(struct peripheral *peripheral,
+					struct diag_cntl_hdr *hdr, size_t len)
+{
+	struct diag_cntl_cmd_diag_id *pkt = to_cmd_diag_id(hdr);
+	struct diag_cntl_cmd_diag_id *ctrl_pkt;
+	char *process_name = NULL;
+	char *root_str = NULL;
+	uint8_t local_diag_id = 0;
+	static uint8_t diag_id = DIAG_ID_APPS;
+	uint32_t version = 0, feature_len = 0;
+	uint8_t new_request = 0;
+	int pd_val, ret;
+	process_name = (char*)&pkt->process_name;
+
+	ret = diag_query_diag_id(process_name, &local_diag_id);
+	if (!ret) {
+		diag_id++;
+		new_request = 1;
+		pd_val = diag_query_pd(process_name);
+		diag_add_diag_id_to_list(diag_id, process_name,
+		pd_val, peripheral);
+		local_diag_id = diag_id;
+	}
+
+	root_str = strstr(process_name, DIAG_ID_ROOT_STRING);
+	ctrl_pkt = malloc(sizeof(*ctrl_pkt));
+
+	if (!ctrl_pkt)
+		return -ENOMEM;
+
+	ctrl_pkt->diag_id = local_diag_id;
+	ctrl_pkt->hdr.cmd = DIAG_CNTL_CMD_DIAG_ID;
+	ctrl_pkt->version = DIAGID_VERSION_1;
+	strncpy((char *)&ctrl_pkt->process_name, process_name,
+		sizeof(ctrl_pkt->process_name));
+	ctrl_pkt->hdr.len = sizeof(ctrl_pkt->diag_id) + sizeof(ctrl_pkt->version) +
+		strlen(process_name) + 1;
+	len = ctrl_pkt->hdr.len+ sizeof(ctrl_pkt->hdr);
+
+	queue_push(&peripheral->cntlq, ctrl_pkt, len);
+
+	return 0;
+}
+
 void diag_cntl_set_diag_mode(struct peripheral *perif, bool real_time)
 {
 	struct diag_cntl_cmd_diag_mode_v2 pkt_v2;
@@ -583,9 +745,12 @@ int diag_cntl_recv(struct peripheral *peripheral, const void *buf, size_t n)
 		case DIAG_CNTL_CMD_DEREGISTER:
 			diag_cntl_deregister(peripheral, hdr, n);
 			break;
+		case DIAG_CNTL_CMD_DIAG_ID:
+			diag_cntl_process_diag_id(peripheral, hdr, n);
+			break;
 		default:
 			warnx("[%s] unsupported control packet: %d",
-			      peripheral->name, hdr->cmd);
+				peripheral->name, hdr->cmd);
 			print_hex_dump("CNTL", buf, n);
 			break;
 		}
diff --git a/router/diag_cntl.h b/router/diag_cntl.h
index f73f0d2..42a3e4b 100644
--- a/router/diag_cntl.h
+++ b/router/diag_cntl.h
@@ -35,6 +35,13 @@
 #include "peripheral.h"
 #include "masks.h"
 
+#define DIAG_ID_ROOT_STRING	"root"
+#define DIAGID_VERSION_1	1
+#define DIAGID_VERSION_2	2
+#define MAX_DIAGID_STR_LEN	30
+#define MIN_DIAGID_STR_LEN	5
+
+
 int diag_cntl_recv(struct peripheral *perif, const void *buf, size_t len);
 void diag_cntl_send_log_mask(struct peripheral *peripheral, uint32_t equip_id);
 void diag_cntl_send_msg_mask(struct peripheral *peripheral, struct diag_ssid_range_t *range);
diff --git a/router/dm.c b/router/dm.c
index b669bef..ede519a 100644
--- a/router/dm.c
+++ b/router/dm.c
@@ -35,6 +35,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
+#include <error.h>
 
 #include "diag.h"
 #include "dm.h"
@@ -59,9 +60,100 @@ struct diag_client {
 
 	struct list_head outq;
 	struct list_head node;
+	struct watch_flow *flow;
 };
 
 struct list_head diag_clients = LIST_INIT(diag_clients);
+struct list_head diag_sock_clients = LIST_INIT(diag_sock_clients);
+
+
+int diag_socket_write(unsigned char *buf, int len, struct watch_flow *flow)
+{
+	if(socket_dm)
+		dm_send_flow(socket_dm, buf, len, flow);
+	return 0;
+}
+
+int diag_md_write(unsigned char *buf, int len, struct watch_flow *flow)
+//int diag_md_write(const void *buf, int len, struct watch_flow *flow)
+{
+	//printf("ODL Write Call\n");
+	struct diag_client *dm;
+	struct list_head *item;
+	size_t outlen;
+	void *outbuf;
+	int user_space_data_type = 32;
+	int remote_proc, data_len;
+	unsigned char *ptr;
+	int num_data_fields = 1, offset = 0;
+
+	outbuf = hdlc_encode(buf, len, &outlen);
+	if (!outbuf) {
+		warn("failed to allocate hdlc destination buffer\n");
+		return -1;
+	}
+
+	data_len = (outlen + sizeof(int)*3);
+	ptr = alloca(data_len);
+	if (!ptr) {
+		warn("diag: In %s failed to allocate memory\n", __func__);
+		return -1;
+	}
+	memcpy(ptr,&user_space_data_type, sizeof(user_space_data_type));
+	offset = offset + sizeof(user_space_data_type);
+	memcpy(ptr + offset, &num_data_fields, sizeof(num_data_fields));
+	offset = offset + sizeof(num_data_fields);
+	memcpy(ptr + offset, &outlen, sizeof(int));
+	offset = offset + sizeof(int);
+	memcpy(ptr + offset, outbuf, outlen);
+
+	list_for_each(item, &diag_sock_clients) {
+		dm = container_of(item, struct diag_client, node);
+
+		dm_send_flow(dm, ptr, data_len, flow);
+	}
+
+	return 0;
+}
+
+static struct diag_logger_ops socket_log_ops = {
+	.write = diag_socket_write,
+	.open = NULL,
+	.close = NULL,
+	.open_device = NULL,
+	.close_device = NULL,
+};
+
+static struct diag_logger_ops md_log_ops = {
+	.write = diag_md_write,
+	.open = NULL,
+	.close = NULL,
+	.open_device = NULL,
+	.close_device = NULL,
+};
+
+
+struct diag_logger_t *logger = NULL;
+
+void diag_mux_init()
+{
+	logger = (struct diag_logger_t *) malloc(sizeof(struct diag_logger_t *));
+	if(logger == NULL)
+		warn("Failed to allocate Memory.\n");
+
+	if(socket_dm)
+		logger->log_ops = &socket_log_ops;
+	else
+	logger->log_ops = &md_log_ops;
+}
+
+void diag_close_logging_process()
+{
+        if(socket_dm)
+                logger->log_ops = &socket_log_ops;
+        else
+                logger->log_ops = &md_log_ops;
+}
 
 /**
  * dm_add() - register new DM
@@ -78,14 +170,32 @@ struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool hdlc_en
 	dm->name = strdup(name);
 	dm->in_fd = in_fd;
 	dm->out_fd = out_fd;
+	dm->flow = NULL;
 	dm->hdlc_encoded = hdlc_encoded;
 	list_init(&dm->outq);
 
-	if (dm->in_fd >= 0)
+	if ((dm->in_fd >= 0) && (dm->name != NULL)) {
+		if (!strcmp(dm->name, "UNIX")) {
+			dm->flow = watch_flow_new();
+			watch_add_readfd(dm->in_fd, dm_recv, dm, dm->flow);
+		} else {
 		watch_add_readfd(dm->in_fd, dm_recv, dm, NULL);
+		}
+	}
 	watch_add_writeq(dm->out_fd, &dm->outq);
 
-	list_add(&diag_clients, &dm->node);
+
+	if (dm->name != NULL) {
+		if (!strcmp(dm->name,"UNIX"))
+			list_add(&diag_sock_clients, &dm->node);
+		else
+			list_add(&diag_clients, &dm->node);
+	} else {
+		free(dm);
+		return NULL;
+	}
+
+//	list_add(&diag_clients, &dm->node);
 
 	/* Disable DM by default, so that  */
 	dm->enabled = false;
@@ -123,31 +233,88 @@ static int dm_recv_hdlc(struct diag_client *dm)
 	return ret;
 }
 
+#define USER_SPACE_DATA_TYPE 0x00000020
+#define USER_SPACE_RAW_DATA_TYPE 0x00000080
+#define USER_SPACE_LOG_EVENT  0x00000010
+
 static int dm_recv_raw(struct diag_client *dm)
 {
-	int saved_errno;
+	int saved_errno,fd;
 	unsigned char buf[4096];
 	ssize_t n;
+	struct hdlc_decoder recv_decoder;
+	struct circ_buf recv_buf;
+	size_t msglen;
+	void *msg;
+	struct dm_pkt{
+		int type;
+		unsigned char pkt;
+	};
+	struct dm_pkt *dmpkt;
 
 	for (;;) {
 		n = read(dm->in_fd, buf, sizeof(buf));
 		if (!n) {
+			fd = dm->in_fd;
 			watch_remove_fd(dm->in_fd);
+			dm_del(dm->in_fd);
+			diag_close_logging_process();
+			close(fd);
 			break;
 		} else if (n < 0 && errno == EAGAIN) {
 			break;
 		} else if (n < 0) {
-			saved_errno = -errno;
 			warn("Failed to read from %s\n", dm->name);
-			return saved_errno;
+			fd = dm->in_fd;
+			watch_remove_fd(dm->in_fd);
+ 	                dm_del(dm->in_fd);
+			diag_close_logging_process();
+			close(fd);
+			break;
+		}
+		dmpkt = (struct dm_pkt *)buf;
+		switch (dmpkt->type) {
+		case USER_SPACE_DATA_TYPE:
+			memset(&recv_decoder, 0, sizeof(recv_decoder));
+			memcpy(recv_buf.buf, buf+4, n-4);
+			recv_buf.tail = 0;
+			recv_buf.head = n-4;
+			msg = hdlc_decode_one(&recv_decoder, &recv_buf, &msglen);
+			diag_client_handle_command(dm, msg, msglen);
+		break;
+		case USER_SPACE_RAW_DATA_TYPE:
+			diag_client_handle_command(dm, buf + 4, n - 4);
+		break;
+		case USER_SPACE_LOG_EVENT:
+			logger->log_ops = &md_log_ops;
+		break;	
+	
+		default:
+			break;
 		}
-
-		diag_client_handle_command(dm, buf, n);
 	}
-
 	return 0;
 }
 
+int dm_del(int fd)
+{
+        struct diag_client *dm;
+        struct list_head *item;
+
+        list_for_each(item, &diag_sock_clients) {
+                dm = container_of(item, struct diag_client, node);
+                if (dm->in_fd == fd) {
+                        if (dm->flow)
+                                free(dm->flow);
+                        list_del(&dm->node);
+                        free(dm);
+                }
+        }
+
+        return 0;
+}
+
+
 /**
  * dm_recv() - read and handle data from a DM
  * @fd:		the file descriptor associated with the DM
@@ -163,7 +330,7 @@ int dm_recv(int fd, void* data)
 		return dm_recv_raw(dm);
 }
 
-static ssize_t dm_send_flow(struct diag_client *dm, const void *ptr, size_t len,
+ssize_t dm_send_flow(struct diag_client *dm, const void *ptr, size_t len,
 			    struct watch_flow *flow)
 {
 	if (!dm->enabled)
@@ -194,16 +361,11 @@ ssize_t dm_send(struct diag_client *dm, const void *ptr, size_t len)
  * @len:	length of message
  * @flow:	flow control context for the peripheral
  */
-void dm_broadcast(const void *ptr, size_t len, struct watch_flow *flow)
+void dm_broadcast(const void *buf, size_t len, struct watch_flow *flow)
 {
-	struct diag_client *dm;
-	struct list_head *item;
-
-	list_for_each(item, &diag_clients) {
-		dm = container_of(item, struct diag_client, node);
-
-		dm_send_flow(dm, ptr, len, flow);
-	}
+	unsigned char *tmp;
+	tmp = (unsigned char *)buf;
+	logger->log_ops->write(tmp, len, flow);
 }
 
 void dm_enable(struct diag_client *dm)
diff --git a/router/dm.h b/router/dm.h
index 9c2195c..985a929 100644
--- a/router/dm.h
+++ b/router/dm.h
@@ -39,6 +39,7 @@ struct diag_client;
 struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool hdlc_encoded);
 int dm_recv(int fd, void* data);
 ssize_t dm_send(struct diag_client *dm, const void *ptr, size_t len);
+ssize_t dm_send_flow(struct diag_client *dm, const void *ptr, size_t len, struct watch_flow *flow);
 void dm_broadcast(const void *ptr, size_t len, struct watch_flow *flow);
 void dm_enable(struct diag_client *dm);
 void dm_disable(struct diag_client *dm);
diff --git a/router/peripheral-qrtr.c b/router/peripheral-qrtr.c
index f3c1537..465b0da 100644
--- a/router/peripheral-qrtr.c
+++ b/router/peripheral-qrtr.c
@@ -272,7 +272,7 @@ void qrtr_perif_close(struct peripheral *perif)
 {
 }
 
-static int qrtr_perif_init_subsystem(const char *name, int instance_base)
+static int qrtr_perif_init_subsystem(const char *name, int instance_base, int id)
 {
 	struct peripheral *perif;
 	struct watch_flow *flow;
@@ -280,12 +280,12 @@ static int qrtr_perif_init_subsystem(const char *name, int instance_base)
 	perif = calloc(1, sizeof(*perif));
 
 	flow = watch_flow_new();
-
 	perif->name = strdup(name);
 	perif->send = qrtr_perif_send;
 	perif->close = qrtr_perif_close;
 	perif->sockets = true;
 	perif->flow = flow;
+	perif->periph_id = id;
 
 	list_init(&perif->cmdq);
 	list_init(&perif->cntlq);
@@ -327,12 +327,12 @@ static int qrtr_perif_init_subsystem(const char *name, int instance_base)
 
 int peripheral_qrtr_init(void)
 {
-	qrtr_perif_init_subsystem("modem", DIAG_INSTANCE_BASE_MODEM);
-	qrtr_perif_init_subsystem("lpass", DIAG_INSTANCE_BASE_LPASS);
+	qrtr_perif_init_subsystem("modem", DIAG_INSTANCE_BASE_MODEM, PERIPHERAL_MODEM);
+/*	qrtr_perif_init_subsystem("lpass", DIAG_INSTANCE_BASE_LPASS);
 	qrtr_perif_init_subsystem("wcnss", DIAG_INSTANCE_BASE_WCNSS);
 	qrtr_perif_init_subsystem("sensors", DIAG_INSTANCE_BASE_SENSORS);
 	qrtr_perif_init_subsystem("cdsp", DIAG_INSTANCE_BASE_CDSP);
 	qrtr_perif_init_subsystem("wdsp", DIAG_INSTANCE_BASE_WDSP);
-
+*/
 	return 0;
 }
diff --git a/router/peripheral.c b/router/peripheral.c
index 1b63a58..9de656c 100644
--- a/router/peripheral.c
+++ b/router/peripheral.c
@@ -63,7 +63,7 @@ void peripheral_close(struct peripheral *peripheral)
 
 int peripheral_init(void)
 {
-	peripheral_rpmsg_init();
+//	peripheral_rpmsg_init();
 	peripheral_qrtr_init();
 
 	return 0;
diff --git a/router/peripheral.h b/router/peripheral.h
index d04c8f8..f4cb46e 100644
--- a/router/peripheral.h
+++ b/router/peripheral.h
@@ -33,6 +33,26 @@
 
 struct diag_ssid_range_t;
 
+#define PERIPHERAL_APPS		0
+#define PERIPHERAL_MODEM	1
+
+#define PERIPHERAL_LPASS	2
+#define PERIPHERAL_WCNSS	3
+#define PERIPHERAL_SENSORS	4
+#define PERIPHERAL_WDSP		5
+#define PERIPHERAL_CDSP		6
+#define PERIPHERAL_NPU		7
+#define NUM_PERIPHERALS		8
+
+#define UPD_WLAN		8
+#define UPD_AUDIO		9
+#define UPD_SENSORS		10
+#define UPD_CHARGER		11
+#define NUM_UPD			4
+#define PD_UNKNOWN		255
+
+
+
 int peripheral_init(void);
 void peripheral_close(struct peripheral *peripheral);
 
diff --git a/router/socket.c b/router/socket.c
index f8f5b0f..f71b344 100644
--- a/router/socket.c
+++ b/router/socket.c
@@ -49,11 +49,12 @@
 #include "watch.h"
 
 #define APPS_BUF_SIZE 16384
+struct diag_client *socket_dm = NULL;
 
 int diag_sock_connect(const char *hostname, unsigned short port)
 {
 	struct sockaddr_in addr;
-	struct diag_client *dm;
+//	struct diag_client *dm;
 	struct hostent *host;
 	int ret;
 	int fd;
@@ -81,8 +82,8 @@ int diag_sock_connect(const char *hostname, unsigned short port)
 
 	printf("Connected to %s:%d\n", hostname, port);
 
-	dm = dm_add("DIAG CLIENT", fd, fd, true);
-	dm_enable(dm);
+	socket_dm = dm_add("Socket Client", fd, fd, true);
+	dm_enable(socket_dm);
 
 	return fd;
 }
diff --git a/router/unix.c b/router/unix.c
index ead9fe0..5b3e41d 100644
--- a/router/unix.c
+++ b/router/unix.c
@@ -85,7 +85,7 @@ int diag_unix_open(void)
 		return -1;
 	}
 
-	ret = listen(fd, 2);
+	ret = listen(fd, 50);
 	if (ret < 0) {
 		fprintf(stderr, "failed to listen on diag socket\n");
 		return -1;
diff --git a/tools/diag-log_on_device.c b/tools/diag-log_on_device.c
new file mode 100644
index 0000000..0be46af
--- /dev/null
+++ b/tools/diag-log_on_device.c
@@ -0,0 +1,656 @@
+/*
+ * Copyright (c) 2021 The Linux Foundation. All rights reserved.
+
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above
+      copyright notice, this list of conditions and the following
+      disclaimer in the documentation and/or other materials provided
+      with the distribution.
+    * Neither the name of The Linux Foundation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <sys/time.h>
+#include <pthread.h>
+#include <err.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <netdb.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <time.h>
+#include <pthread.h>
+#include <stdint.h>
+#include <signal.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <dirent.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <sys/select.h>
+#include <ctype.h>
+#include <limits.h>
+#include <stdlib.h>
+
+#define FILE_LIST_NAME_SIZE 100
+#define MAX_FILES_IN_FILE_LIST 100
+#define std_strlprintf snprintf
+#define LOG_FILENAME_PREFIX_LEN 9
+#define READ_BUF_SIZE 100000
+#define DISK_BUF_SIZE 8192
+#define CONTROL_CHAR 0x7E
+#define USER_SPACE_DATA_TYPE 0x00000020
+#define USER_SPACE_RAW_DATA_TYPE 0x00000080
+#define USER_SPACE_LOG_EVENT     0x00000010
+#define FILE_NAME_LEN 500
+#define MASK_FILE_BUF_SIZE 8192
+
+
+
+struct buffer_pool {
+	unsigned int bytes_in_buff;
+	unsigned char buffer_ptr[DISK_BUF_SIZE];
+};
+
+/*Static declaration of buffer. */
+struct buffer_pool pools[] = {
+	[0] = {
+		.bytes_in_buff = 0,
+	},
+	[1] = {
+		.bytes_in_buff = 0,
+	},
+
+};
+
+typedef uint32_t	uint32;
+typedef uint8_t		uint8;
+
+pthread_cond_t cond1 = PTHREAD_COND_INITIALIZER;
+pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
+
+/* Thread Handlers */
+pthread_t read_thread_hdl;	/* Diag Read thread handle */
+pthread_t write_thread_hdl;	/* Diag disk write thread handle */
+
+/* Global Declarations */
+int curr_read = 0;
+int curr_write = 0;
+unsigned char read_buffer[READ_BUF_SIZE];
+char output_dir[FILE_NAME_LEN] = {"/tmp"};
+char mask_clear_file[] = "/tmp/diag_mask.cfg";
+int fd_dev = -1; // Socket Descriptor
+int fd_device = -1; //FIle Descriptor
+char timestamp_buf[30];
+char file_name_curr[FILE_NAME_LEN];
+static char *open_mask_file= NULL;
+static int diag_mask_file = 0;
+static int diag_timeout;
+int num_bytes_read;
+unsigned int max_file_num = 10;
+static unsigned int file_count = 0;
+int file_list_size;
+unsigned long max_file_size = 100000000;
+unsigned long min_file_size = 80000000;
+unsigned static long count_written_bytes;
+char* file_list = NULL;
+int file_list_index = -1;
+char file_name_del[FILE_NAME_LEN];
+int overflow_flag = 0;
+int cleanup_mask;
+
+int delete_log(void);
+
+/* Function to get timestamp */
+void get_time_string(char *buffer, int len)
+{
+	struct timeval tv;
+	struct tm *tm;
+	unsigned long long milliseconds = 0;
+	char timestamp_buf[30];
+
+	if (!buffer || len <= 0)
+		return;
+
+	gettimeofday(&tv, NULL);
+	tm = localtime(&tv.tv_sec);
+	if (!tm)
+		return;
+
+	milliseconds = (tv.tv_sec * 1000LL) + (tv.tv_usec / 1000);
+	strftime(timestamp_buf, 30, "%Y%m%d_%H%M%S", tm);
+	(void)snprintf(buffer, len, "%s%lld",
+		timestamp_buf, milliseconds);
+}
+
+static void usage(void)
+{
+	fprintf(stderr,
+		"User space application for diag interface\n"
+		"\n"
+		"usage: diag [-of]\n"
+		"\n"
+		"options:\n"
+		"-o	<output directory>\n"
+		"-f	<Diag Configuration file>\n"
+		"-t	<Log Timeout>\n"
+		"-n	<number of log files>\n"
+		"-s	<Size in MB>\n"
+		"-c	<Cleanup Modem Mask>\n"
+	);
+	exit(1);
+}
+
+
+int send_empty_mask(int type)
+{
+	int fd_mask, ch, ret;
+	const uint8 size = 40;
+	unsigned char mask_buf[size];
+	int count_mask_bytes = 4;
+	*(int *)mask_buf = USER_SPACE_RAW_DATA_TYPE;
+	FILE *mask_fp = NULL;
+
+	if ((mask_fp = fopen(mask_clear_file, "rb")) == NULL) {
+		printf("can't open mask clear file: %s, errno: %d\n", mask_clear_file,errno);
+		return -1;
+	}
+	while(1)
+	{
+		ch = fgetc(mask_fp);
+		if(ch == EOF)
+			break;
+
+		mask_buf[count_mask_bytes] = ch;
+		if (mask_buf[count_mask_bytes] == CONTROL_CHAR) {
+			ret = write(fd_dev, mask_buf, count_mask_bytes-1);
+			if(ret < 0) {
+				printf("Write Error on the Socket: errno = %d\n", errno);
+				return -1;
+			}
+		*(int *)mask_buf = USER_SPACE_DATA_TYPE;
+		count_mask_bytes = 4;
+		} else {
+			count_mask_bytes++;
+		}
+	}
+	fclose(mask_fp);
+
+	return 0;	
+}
+
+
+static void log_timeout(int sig, siginfo_t *siginfo, void *context)
+{
+	int ret;
+	printf ("Sending PID: %ld, UID: %ld\n",
+			(long)siginfo->si_pid, (long)siginfo->si_uid);
+	ret = send_empty_mask(0);
+	if(ret < 0)
+		printf("Couldn't open Mask clear file.\n");
+
+	close(fd_device);
+	close(fd_dev);
+	pthread_cancel(read_thread_hdl);
+	pthread_cancel(write_thread_hdl);
+}
+int close_logging_file()
+{
+	int status;
+	char timestamp_buf[30];
+	char new_filename[FILE_NAME_LEN];
+
+	close(fd_device);
+	/* check whether number of files in /tmp. Delete if we exceed the limit */
+	if(file_count > max_file_num) {
+		printf("Delete Oldest File.\n");
+		if (!delete_log()) {
+			file_count--;
+		}
+	}
+	get_time_string(timestamp_buf, sizeof(timestamp_buf));
+	snprintf(new_filename,
+			FILE_NAME_LEN, "%s%s%s%s",
+			output_dir,"/diag_log_",
+			timestamp_buf, ".qmdl");
+
+	fd_device = open(new_filename,
+			O_CREAT | O_RDWR | O_SYNC | O_TRUNC,
+			0644);
+	if(fd_device < 0) {
+		printf(" File open error: %d\n", errno);
+		return -1;
+	}
+	else {
+		file_count++; 
+	}
+	strncpy(file_name_curr, new_filename, FILE_NAME_LEN);
+	return 0;
+}
+
+static void *WriteThread(void *data)
+{
+	unsigned int chunks, last_chunk;
+	struct stat finfo;
+	int ret;
+	while(1) {
+		pthread_mutex_lock(&lock);
+		pthread_cond_wait(&cond1, &lock);
+		ret = write(fd_device,pools[curr_write].buffer_ptr,DISK_BUF_SIZE);
+		if(ret > 0) {
+			pools[curr_write].bytes_in_buff = 0;
+		} else if (ret < 0) {
+			goto fail;
+		}
+		/* Check whether we reached to MAX size of the File */
+		if(fstat(fd_device, &finfo) == 0) {
+			if(finfo.st_size > min_file_size) {
+				ret = close_logging_file();
+				if(ret < 0)
+					goto fail;
+				count_written_bytes = 0;
+			}
+		} 
+		else {
+			goto fail;
+		}
+		pthread_mutex_unlock(&lock);
+		curr_write =! curr_write;
+	}
+fail: 
+	pthread_cancel(read_thread_hdl);
+	close(fd_dev);
+	close(fd_device);
+	return (void *)0;
+}
+/* Read Thread */
+static void *CreateWaitThread(void* data) 
+{
+	int read_bytes = 0,type,num_data_fields;
+	uint32 count_received_bytes;
+	unsigned char* ptr;
+
+	while(1) {
+		num_bytes_read = 0;
+		memset(read_buffer, 0, READ_BUF_SIZE);
+		num_bytes_read = read(fd_dev, (void*)read_buffer,READ_BUF_SIZE);
+		if(!num_bytes_read || (num_bytes_read < 0))
+			goto read_failure;
+
+		type = *(int *)read_buffer;
+		ptr = read_buffer+4;
+		num_data_fields = *(int *)ptr;
+		ptr += 4;
+		count_received_bytes = *(uint32*)ptr;
+		ptr += sizeof(uint32);
+		count_written_bytes += num_bytes_read;
+		if(count_received_bytes >= (DISK_BUF_SIZE - pools[curr_read].bytes_in_buff)) {
+		/* Trigger Write Thread */
+			curr_read =! curr_read;
+			pthread_cond_signal(&cond1);
+		}
+
+		if(count_received_bytes > 0) {/* Buffer space is available */
+			memcpy(pools[curr_read].buffer_ptr + pools[curr_read].bytes_in_buff, ptr, count_received_bytes);
+			pools[curr_read].bytes_in_buff += count_received_bytes;
+		}
+	}
+
+read_failure:
+	close(fd_dev);
+	close(fd_device);
+	pthread_cancel(write_thread_hdl);
+	return (void *)0;	
+}
+
+static int create_oldest_file_list(char *oldest_dir)
+{
+
+	int status = 1;
+	struct dirent **dirent_list;
+	int i,n,type=0;
+	int num_entries = 0;
+	int num_entries_capped = 0;
+	char *name_ptr;
+	int num_bytes = 0;
+
+	num_entries = scandir(oldest_dir, &dirent_list, 0,(int(*)(const struct dirent **, const struct dirent **))alphasort);
+	if(!dirent_list) {
+		printf("In %s, couldn't get the dirent_list, errno: %d, directory: %s\n",
+		__func__, errno, oldest_dir);
+		return 0;
+	} else if (num_entries < 0) {
+		printf("In %s, error determining directory entries, errno: %d, directory: %s\n",
+			__func__, errno, oldest_dir);
+		return 0;
+	}
+
+	/* Limit the size of the list so we aren't working with too many files */
+	num_entries_capped = (num_entries > MAX_FILES_IN_FILE_LIST) ?
+				MAX_FILES_IN_FILE_LIST : num_entries;
+		
+	if (num_entries_capped - 2 > 0) {
+		file_list_size = num_entries_capped - 2;
+		num_bytes = FILE_LIST_NAME_SIZE * file_list_size;
+		file_list = malloc(num_bytes);
+	}
+
+	if (file_list) {
+		file_list_index = 0;
+		for (i = 0; i < num_entries_capped; i++)
+		{
+			if ((strncmp(dirent_list[i]->d_name, "diag_log_",LOG_FILENAME_PREFIX_LEN) != 0))
+				continue;
+			if (file_list_index < file_list_size)
+			{
+				name_ptr = file_list +
+						(file_list_index * FILE_LIST_NAME_SIZE);
+				strncpy(name_ptr, dirent_list[i]->d_name, FILE_LIST_NAME_SIZE);
+				*(name_ptr + (FILE_LIST_NAME_SIZE - 1)) = 0;
+					file_list_index++;
+			}
+			if (file_list_index > 0) {
+				if (file_list_index < file_list_size) {
+					int new_size = FILE_LIST_NAME_SIZE *file_list_index;
+					char *temp_ptr = realloc(file_list, new_size);
+					if (temp_ptr)
+						file_list = temp_ptr;
+				}
+			file_list_size = file_list_index;
+			}
+		}
+	}
+	else if (num_bytes > 0) {
+		printf("Memory Allocation error.\n");
+		status = 0;
+	}
+	
+	i = num_entries;
+	while (i--) {
+	free(dirent_list[i]);
+	}
+	
+	free(dirent_list); 
+	return status;
+}
+
+static int get_oldest_file(char* oldest_file, char *oldest_dir)
+{
+	int status = 0;
+		status = create_oldest_file_list(oldest_dir);
+
+	if (file_list) {
+		if (oldest_file) {
+			strncpy(oldest_file, file_list,
+				FILE_LIST_NAME_SIZE);
+			file_list_index++;
+			status = 1;
+		} else {
+			printf("In %s, oldest_file is NULL\n", __func__);
+		}
+		
+	}else {
+		status =0;
+		printf("No Log files in the dicrectory.\n");
+	}
+
+	return status;
+}
+/* Number of Log file in /tmp directory */
+static int get_file_count(char *oldest_dir)
+{
+	struct dirent **dirent_list;
+	int i,num_entries = 0;
+	int num_entries_capped = 0;
+
+	num_entries = scandir(oldest_dir, &dirent_list, 0,(int(*)(const struct dirent **, const struct dirent **))alphasort);
+	if(!dirent_list) {
+		printf("In %s, couldn't get the dirent_list, errno: %d, directory: %s\n",
+			__func__, errno, oldest_dir);
+		return 0;
+	} else if (num_entries < 0) {
+		printf("In %s, error determining directory entries, errno: %d, directory: %s\n",
+			 __func__, errno, oldest_dir);
+		return 0;
+	}
+	
+	for (i = 0; i < num_entries; i++) {
+		if ((strncmp(dirent_list[i]->d_name, "diag_log_",LOG_FILENAME_PREFIX_LEN) != 0))
+			continue;
+	file_count++;
+	}
+	return file_count;
+}
+
+int delete_log()
+{
+	int status;
+	char oldest_file[FILE_LIST_NAME_SIZE] = "";
+	struct stat file_stat;
+
+	status = get_oldest_file(oldest_file,
+					output_dir);
+	if (0 == status) {
+		printf("diag: In %s, Unable to determine oldest file for deletion\n",
+			 __func__);
+		return -1;
+	}
+	std_strlprintf(file_name_del,
+			FILE_NAME_LEN, "%s%s%s",
+			output_dir, "/", oldest_file);
+	if (!strncmp(file_name_curr, file_name_del, FILE_NAME_LEN)) {
+		printf("diag: In %s, Cannot delete file, file %s is in use \n",
+			 __func__, file_name_curr);
+		return -1;
+	}
+	stat(file_name_del, &file_stat);
+	/* Convert size to KB */
+	file_stat.st_size /= 1024;
+	if (unlink(file_name_del)) {
+		printf("In %s, Unable to delete file: %s, errno: %d\n",
+			__func__, file_name_del, errno);
+		return -1;
+	} else {
+		printf("In %s, Deleting logfile %s of size %lld KB\n",
+			__func__, file_name_del,
+			(long long int) file_stat.st_size);
+		free(file_list);
+	}
+	return 0;
+}
+
+
+int main(int argc, char **argv)
+{
+	int ret;
+	int c,ch,found_cmd;
+	struct sockaddr_un addr;
+	struct timeval tv = {20, 0};
+	struct sigaction act;
+	int count_mask_bytes = 0;
+	unsigned char mask_buf[MASK_FILE_BUF_SIZE];
+	unsigned char mask_log[4];
+	FILE *read_mask_fp;
+	*(int *)mask_buf = USER_SPACE_DATA_TYPE;
+	*(int *)mask_log = USER_SPACE_LOG_EVENT; 
+	
+	count_mask_bytes = 4;
+	memset (&act, '\0', sizeof(act));
+	act.sa_sigaction = &log_timeout;
+	act.sa_flags = SA_SIGINFO;
+	if (sigaction(SIGALRM, &act, NULL) < 0) {
+		perror ("sigaction");
+		return 1;
+	}
+
+	for (;;) {
+		c = getopt(argc, argv, "hf:o:t:s");
+		if (c < 0)
+			break;
+
+		switch (c) {
+		case 'c':
+			cleanup_mask = 1;
+			break;
+		
+		case 's':
+			max_file_size = atol(optarg);
+			if ((long)max_file_size <= 0)
+				max_file_size = 100000000;
+			else {
+				max_file_size *= 1024 * 1024;
+			//	if (max_file_size >= 0 && max_file_size < 1024 * 1024)
+			//		max_file_size = 100000000;
+			}
+			min_file_size = ((max_file_size / 100) * 80);
+			break;
+		
+		case 'o':
+			file_count = get_file_count(output_dir);
+			printf("QMDL File Count in /tmp Directory = %d\n", file_count);
+			
+			if(max_file_num > 1 && (file_count >= max_file_num)) { /* Check file_count before creating the Log File. */
+				printf("In %s, File count reached max file num %u so deleting oldest file\n",__func__, max_file_num);
+			
+			while(file_count > max_file_num)
+				if (!delete_log()) {
+				file_count--;
+				}
+			}
+
+			get_time_string(timestamp_buf, sizeof(timestamp_buf));
+			snprintf(file_name_curr,
+					FILE_NAME_LEN, "%s%s%s%s",
+					output_dir,"/diag_log_",
+					timestamp_buf, ".qmdl");
+			fd_device = open(file_name_curr,
+					O_CREAT | O_RDWR | O_SYNC | O_TRUNC,
+					0644);
+			if(fd_device < 0) {
+				printf(" File open error: %d\n", errno);
+				return -1;
+			}
+			else {
+				file_count++;
+			}
+			printf("QMDL Fil: %s\n", file_name_curr);
+			break;
+
+		case 'f':
+			open_mask_file = strdup(optarg);
+			diag_mask_file = 1;
+			break;
+		case 't':
+			printf("Timeout for QMDL Logging\n");
+			diag_timeout = atoi(optarg);
+			printf("Time out value = %d\n", diag_timeout);
+			alarm(diag_timeout);
+			break;
+		default:
+		case 'h':
+			usage();
+			break;
+		}
+	}
+
+	fd_dev = socket(AF_UNIX, SOCK_SEQPACKET, 0);
+	if (fd_dev < 0)
+		goto failure_case3;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sun_family = AF_UNIX;
+	strncpy(addr.sun_path, "\0diag", sizeof(addr.sun_path)-1);
+	
+	ret = connect(fd_dev, (struct sockaddr*)&addr, sizeof(addr));
+	if (ret < 0)
+		goto failure_case2;
+	
+	ret = write(fd_dev,mask_log,4);
+	if(ret < 0)
+		printf("LOg MAsk failed to write\n");
+
+	pthread_create(&read_thread_hdl, NULL, CreateWaitThread, NULL);
+	if(read_thread_hdl == 0) {
+		printf("Failed to create Read Thread.\n");
+		goto failure_case2;
+	}
+	pthread_create(&write_thread_hdl, NULL, WriteThread, NULL);
+	if(write_thread_hdl == 0) {
+		printf("Failed to create Write Thread.\n");
+		if(read_thread_hdl == 0)
+			pthread_cancel(read_thread_hdl);
+		goto failure_case2;
+	}
+
+	if ((read_mask_fp = fopen(open_mask_file, "rb")) == NULL) {
+		printf("can't open mask file: %s, errno: %d\n", open_mask_file,errno);
+		goto failure_case1;
+	}
+
+	if(diag_mask_file){
+		while(1){
+			ch = fgetc(read_mask_fp);
+			if (ch == EOF)
+			break;  
+			mask_buf[count_mask_bytes] = ch;
+			if (mask_buf[count_mask_bytes] == CONTROL_CHAR) {
+
+			if (!found_cmd)
+				found_cmd = 1;
+			ret = write(fd_dev, mask_buf, count_mask_bytes+1);
+			if(ret < 0) {
+				printf("Write Error on the Socket: errno = %d\n", errno);
+				goto failure_case0;
+			}
+		
+			*(int *)mask_buf = USER_SPACE_DATA_TYPE;
+			count_mask_bytes = 4;
+			} else {
+			count_mask_bytes++;
+			}
+		}
+		if(!found_cmd){
+			printf("No command found:\n");
+		}
+	}
+	while(1)
+		sleep(3600);
+
+failure_case0:
+	fclose(read_mask_fp);
+
+failure_case1:
+	pthread_cancel(read_thread_hdl);
+	pthread_cancel(write_thread_hdl);
+
+failure_case2:
+	close(fd_dev);
+
+failure_case3:
+	close(fd_device);
+
+	return 0;
+}
diff --git a/tools/send_data.c b/tools/send_data.c
index 95ac274..84a338b 100644
--- a/tools/send_data.c
+++ b/tools/send_data.c
@@ -47,6 +47,10 @@
 #define DIAG_CMD_RSP_BAD_COMMAND			0x13
 #define DIAG_CMD_RSP_BAD_PARAMS				0x14
 #define DIAG_CMD_RSP_BAD_LENGTH				0x15
+#define USER_SPACE_RAW_DATA_TYPE                        0x00000080
+#define USER_SPACE_LOG_EVENT                            0x00000010
+
+typedef uint32_t        uint32;
 
 int main(int argc, char **argv)
 {
@@ -58,12 +62,31 @@ int main(int argc, char **argv)
 	char buf[8192];
 	int ret;
 	int fd;
-	int i;
+	int i,j=1;
+	unsigned char *req_modem_loopback;
+	unsigned char *req_modem_logging;
+	int REQ_LOOPBACK_LEN;
+	unsigned char* ptr;
+	uint32 count_received_bytes;
+	(void)argc;
+	(void)argv;
+	
+	REQ_LOOPBACK_LEN = argc -1 + 4;
+
+	printf("REQ_LOOPBACK_LEN = %d\n", REQ_LOOPBACK_LEN);
+
+	req_modem_loopback = calloc(argc - 1 + 4, sizeof(*req_modem_loopback));
+	if (!req_modem_loopback)
+		exit(1);
+	*(int *)req_modem_loopback = USER_SPACE_RAW_DATA_TYPE;
+
+	req_modem_logging = calloc(4, sizeof(*req_modem_loopback));
+
+	*(int *)req_modem_logging = USER_SPACE_LOG_EVENT;
 
-	msg = calloc(argc - 1, sizeof(*msg));
+	for (i = 4; i < REQ_LOOPBACK_LEN; i++,j++)
+		req_modem_loopback[i] = atoi(argv[j]);
 
-	for (i = 1; i < argc; i++)
-		msg[i - 1] = atoi(argv[i]);
 
 	fd = socket(AF_UNIX, SOCK_SEQPACKET, 0);
 	if (fd < 0)
@@ -76,8 +99,12 @@ int main(int argc, char **argv)
 	ret = connect(fd, (struct sockaddr*)&addr, sizeof(addr));
 	if (ret < 0)
 		err(1, "failed to connect to diag");
+	
+	n = write(fd,req_modem_logging,4);
+	if(n < 0)
+		err(1, "failed to send request");
 
-	n = write(fd, msg, argc - 1);
+	n = write(fd, req_modem_loopback, REQ_LOOPBACK_LEN);
 	if (n < 0)
 		err(1, "failed to send request");
 
@@ -108,12 +135,25 @@ int main(int argc, char **argv)
 			printf("Diag response: Bad command\n");
 			break;
 		}
-
+		
+		ptr = (unsigned char*)buf;
+                ptr = ptr+4;
+                ptr += 4;
+                count_received_bytes = *(uint32*)ptr;
+                ptr += sizeof(uint32);
+#if 0
 		for (i = 0; i < n; i++) {
 			printf("%s%d", i == 0 ? "" : " ", buf[i]);
 			if (i % 16 == 15 || i == n - 1)
 				printf("\n");
 		}
+#endif
+	 for (i = 0; i < count_received_bytes; i++) {
+		if (i % 8 == 0) {
+			printf("\n  ");
+		}
+		printf("%02x ", ptr[i]);
+	}
 
 		if (buf[0] == msg[0])
 			break;
-- 
2.7.4