From 750ff77929b6c4795ea8b20ec544f8d275a6bf80 Mon Sep 17 00:00:00 2001
From: Derek Dai <daiderek@gmail.com>
Date: Sat, 18 Feb 2017 18:28:16 +0800
Subject: [PATCH] miracle-*ctl: fix and enhance wfd_subelement parser

---
 res/miracle-wfdctl-demo |  5 ++---
 src/ctl/wfd.c           | 45 ++++++++++++++++++++++++++---------------
 src/ctl/wfdctl.c        |  6 +++---
 3 files changed, 34 insertions(+), 22 deletions(-)

diff --git a/res/miracle-wfdctl-demo b/res/miracle-wfdctl-demo
index efab5e4..32e241d 100755
--- a/res/miracle-wfdctl-demo
+++ b/res/miracle-wfdctl-demo
@@ -16,9 +16,8 @@ if [[ ${#2} != 17 ]]; then
     exit 1
 fi
 
-# since this wfd subelement is used for device info, so no wfd subelemnt
-# id needed (eg. 000600101b4400c8 vs 00000600101b4400c8)
-wfd_subelems=000600101c4400c8
+# this wfd_subelems has no ID (00, device info)
+wfd_subelems=000600111c4400c8
 link_index="_3${1:0:1}${1:1}"
 peer_id="${2//:/_3a}_40$1"
 
diff --git a/src/ctl/wfd.c b/src/ctl/wfd.c
index f5c84ad..5ba7124 100644
--- a/src/ctl/wfd.c
+++ b/src/ctl/wfd.c
@@ -27,6 +27,12 @@
 
 typedef int (*wfd_sube_parse_func)(const char *in, union wfd_sube *out);
 
+struct wfd_sube_info
+{
+	wfd_sube_parse_func parser;
+	uint8_t len;
+};
+
 static int wfd_sube_parse_device_info(const char *in, union wfd_sube *out);
 static int wfd_sube_parse_audio_formats(const char *in, union wfd_sube *out);
 static int wfd_sube_parse_video_formats(const char *in, union wfd_sube *out);
@@ -103,11 +109,11 @@ static const struct wfd_resolution resolutions_hh[] = {
 	{11,  848,	480, 60, 1},	/* p60 */
 };
 
-static const wfd_sube_parse_func parse_func_tbl[WFD_SUBE_ID_RESERVED] = {
-	[WFD_SUBE_ID_DEVICE_INFO] =		wfd_sube_parse_device_info,
-	[WFD_SUBE_ID_AUDIO_FORMATS] =		wfd_sube_parse_audio_formats,
-	[WFD_SUBE_ID_VIDEO_FORMATS] =		wfd_sube_parse_video_formats,
-	[WFD_SUBE_ID_WFD_EXT_CAPS] =		wfd_sube_parse_ext_caps,
+static const struct wfd_sube_info parser_tbl[WFD_SUBE_ID_RESERVED] = {
+	[WFD_SUBE_ID_DEVICE_INFO] = { .parser = wfd_sube_parse_device_info, .len = 6 },
+	[WFD_SUBE_ID_AUDIO_FORMATS] = { .parser = wfd_sube_parse_audio_formats, .len = 15 },
+	[WFD_SUBE_ID_VIDEO_FORMATS] = { .parser = wfd_sube_parse_video_formats, .len = 21 },
+	[WFD_SUBE_ID_WFD_EXT_CAPS] = { .parser = wfd_sube_parse_ext_caps, .len = 2 },
 };
 
 int wfd_get_resolutions(enum wfd_resolution_standard std,
@@ -306,13 +312,8 @@ static int wfd_sube_parse_ext_caps(const char *in, union wfd_sube *out)
 int wfd_sube_parse(const char *in, union wfd_sube *out)
 {
 	uint8_t id;
-	const char *eoi = in + strlen(in);
 	int r;
 
-	if((in + 2) >= eoi) {
-		return -EINVAL;
-	}
-
 	r = sscanf(in, "%2hhx", &id);
 	if(1 > r) {
 		return -EINVAL;
@@ -326,25 +327,37 @@ int wfd_sube_parse_with_id(enum wfd_sube_id id,
 				union wfd_sube *out)
 {
 	uint16_t len;
-	int r = sscanf(in, "%4hx", &len);
+	union wfd_sube sube;
+	int r;
+   
+	if(SHL_ARRAY_LENGTH(parser_tbl) <= id) {
+		return -EINVAL;
+	}
+
+	r = sscanf(in, "%4hx", &len);
 	if(1 > r) {
 		return -EINVAL;
 	}
 
-	if(SHL_ARRAY_LENGTH(parse_func_tbl) <= id) {
+	if(parser_tbl[id].len != len) {
 		return -EINVAL;
 	}
 
-	if(!parse_func_tbl[id]) {
-		return 0;
+	if(!parser_tbl[id].parser) {
+		return -ENOTSUP;
 	}
 
-	r = (*parse_func_tbl[id])(in + 4, out);
+	r = (*parser_tbl[id].parser)(in + 4, &sube);
 	if(0 > r) {
 		return r;
 	}
 
-	out->id = id;
+	sube.id = id;
+
+	if(out) {
+		*out = sube;
+	}
+
 	return r;
 }
 
diff --git a/src/ctl/wfdctl.c b/src/ctl/wfdctl.c
index 390c367..14198f8 100644
--- a/src/ctl/wfdctl.c
+++ b/src/ctl/wfdctl.c
@@ -320,9 +320,9 @@ void ctl_fn_peer_new(struct ctl_peer *p)
 {
 	struct wfd_sink *s;
 	union wfd_sube sube;
-	int r = wfd_sube_parse_with_id(WFD_SUBE_ID_DEVICE_INFO,
-					p->wfd_subelements,
-					&sube);
+	int r;
+
+	r = wfd_sube_parse(p->wfd_subelements, &sube);
 	if(0 > r) {
 		log_debug("invalid subelement: '%s'", p->wfd_subelements);
 		return;