From 20aad66f2fbb535cb51536ca387149f53ffa99fd Mon Sep 17 00:00:00 2001
From: Dumpeti Sathish Kumar <sdumpeti@codeaurora.org>
Date: Mon, 19 Apr 2021 15:37:44 +0530
Subject: [PATCH 3/4] ODL packet formatting support in diag-router

---
 router/dm.c | 44 +++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 39 insertions(+), 5 deletions(-)

diff --git a/router/dm.c b/router/dm.c
index b669bef..b9585ba 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"
@@ -128,6 +129,10 @@ static int dm_recv_raw(struct diag_client *dm)
 	int saved_errno;
 	unsigned char buf[4096];
 	ssize_t n;
+	struct hdlc_decoder recv_decoder;
+	struct circ_buf recv_buf;
+	size_t msglen;
+	void *msg;
 
 	for (;;) {
 		n = read(dm->in_fd, buf, sizeof(buf));
@@ -141,10 +146,13 @@ static int dm_recv_raw(struct diag_client *dm)
 			warn("Failed to read from %s\n", dm->name);
 			return saved_errno;
 		}
-
-		diag_client_handle_command(dm, buf, n);
+		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);
 	}
-
 	return 0;
 }
 
@@ -194,15 +202,41 @@ 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;
+	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) {
+		printf("failed to allocate hdlc destination buffer\n");
+		return;
+	}
+	
+	data_len = (outlen + sizeof(int)*3);
+	ptr = alloca(data_len);
+	if (!ptr) {
+		printf("diag: In %s failed to allocate memory\n", __func__);
+		return;
+	}
+	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_clients) {
 		dm = container_of(item, struct diag_client, node);
 
-		dm_send_flow(dm, ptr, len, flow);
+		dm_send_flow(dm, ptr, data_len, flow);
 	}
 }
 
-- 
2.7.4