HDMIservice: Fix resolution and socket handling.

Resolution priority is updated.
Socket handling is updated.

ST-Ericsson ID: 326691
ST-Ericsson FOSS-OUT ID: STETL-FOSS-OUT-10177

Signed-off-by: Per Persson <per.xb.persson@stericsson.com>
Change-Id: Id943772089d766dde48531eaf9370d132bca1e96
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/23740
Reviewed-by: Per-Daniel OLSSON <per-daniel.olsson@stericsson.com>
Reviewed-by: Jocelyn LEHEUP <jocelyn.leheup@stericsson.com>
Reviewed-by: Jimmy RUBIN <jimmy.rubin@stericsson.com>

4 files changed, 123 insertions, 102 deletions

diff --git a/include/hdmi_service_local.h b/include/hdmi_service_local.h
index 83f2bcb..8e6afdb 100644
--- a/include/hdmi_service_local.h
+++ b/include/hdmi_service_local.h
@@ -82,6 +82,7 @@ struct video_format *video_formats_get(void);
 void set_vesacea_prio_all(void);
 int hdmi_fb_chres(__u8 cea, __u8 vesaceanr);
 int vesaceaprio_set(__u8 len, __u8 *data);
+void vesacea_prio_default(void);
 int hdmievclr(__u8 mask);
 void thread_kevent_fn(void *arg);
 int hdmiplug_subscribe(void);
diff --git a/src/hdmi_service.c b/src/hdmi_service.c
index 2542566..7e4ea08 100644
--- a/src/hdmi_service.c
+++ b/src/hdmi_service.c
@@ -293,7 +293,8 @@ static int hdmiplugged_handle(int *basic_audio_support)
 	while (res && (cnt < 3)) {
 		res = edid_read(0, data);
 		if (res == 0)
-			res = edid_parse0(data + 1, &extension, formats, nr_formats);
+			res = edid_parse0(data + 1, &extension, formats,
+						nr_formats);
 		if (res && (cnt < 2))
 			usleep(EDIDREAD_WAITTIME);
 		cnt++;
@@ -364,7 +365,8 @@ static int hdmiplugged_handle(int *basic_audio_support)
 		lseek(disponoff, 0, SEEK_SET);
 		read_res = read(disponoff, buf, sizeof(buf));
 		if (read_res <= 0) {
-			LOGHDMILIB("***** Failed to read %s *****", DISPONOFF_FILE);
+			LOGHDMILIB("***** Failed to read %s *****",
+						DISPONOFF_FILE);
 			close(disponoff);
 			ret = -5;
 			goto hdmiplugged_handle_end;
@@ -475,7 +477,8 @@ infofr_send_end:
 }
 
 /* Send plug event message on client socket */
-static int plugevent_send(__u32 cmd, int audio_support, int nr, struct vesacea vesacea[])
+static int plugevent_send(__u32 cmd, int audio_support, int nr,
+					struct vesacea vesacea[])
 {
 	int res = 0;
 	int val;
@@ -603,7 +606,6 @@ static int hdmi_eventcmd(void)
 		case HDMI_EDIDREQ:
 		case HDMI_FB_RES_SET:
 		case HDMI_HDCP_INIT:
-		case HDMI_VESACEAPRIO_SET:
 		case HDMI_INFOFR:
 			handlecmd = 0;
 			powerstate_get(&power_state);
@@ -675,7 +677,8 @@ static int hdmi_eventcmd(void)
 			break;
 
 		case HDMI_HDCP_INIT:
-			if (cmd_obj->data_len != (AES_KEYS_SIZE + 12))
+			if (cmd_obj->data_len !=
+					(AES_KEYS_SIZE + CMDBUF_OFFSET))
 				res = -1;
 			else
 				res = hdcp_init(cmd_obj->data);
@@ -710,7 +713,8 @@ static int hdmi_eventcmd(void)
 			break;
 		}
 
-		LOGHDMILIB("cmd_id:%x res:%d\n", cmd_obj->cmd_id, res);
+		LOGHDMILIB("cmd:%d cmd_id:%x res:%d\n", cmd_obj->cmd,
+							cmd_obj->cmd_id, res);
 
 		free(cmd_obj);
 
@@ -834,7 +838,6 @@ static void thread_main_fn(void *arg)
 int hdmi_service_init(int avoid_return_msg)
 {
 	int dummy = 0;
-	__u8 ceavesadata[2];
 	int socket;
 
 	LOGHDMILIB("%s begin", __func__);
@@ -846,10 +849,7 @@ int hdmi_service_init(int avoid_return_msg)
 	/* Set sysfs format to binary */
 	storeastext(0);
 
-	ceavesadata[0] = 0;
-	ceavesadata[1] = 0;
-	LOGHDMILIB("desired cea:%d nr:%d", ceavesadata[0], ceavesadata[1]);
-	vesaceaprio_set(1, ceavesadata);
+	vesacea_prio_default();
 
 	pthread_mutex_init(&event_mutex, NULL);
 	pthread_mutex_init(&cmd_mutex, NULL);
@@ -875,14 +875,14 @@ int hdmi_service_exit(void)
 	__u8 buf[32];
 	val = HDMI_EXIT;
 
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = 0;
-	memcpy(&buf[8], &val, 4);
-	serversocket_write(12 + val, buf);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -893,14 +893,14 @@ int hdmi_service_enable(void)
 	__u8 buf[32];
 	val = HDMI_ENABLE;
 
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = 0;
-	memcpy(&buf[8], &val, 4);
-	serversocket_write(12 + val, buf);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -911,14 +911,14 @@ int hdmi_service_disable(void)
 	__u8 buf[32];
 	val = HDMI_DISABLE;
 
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = 0;
-	memcpy(&buf[8], &val, 4);
-	serversocket_write(12 + val, buf);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -941,17 +941,17 @@ int hdmi_service_resolution_set(int cea, int vesaceanr)
 	__u8 buf[32];
 
 	val = HDMI_FB_RES_SET;
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = 2;
-	memcpy(&buf[8], &val, 4);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
 	/* data */
-	buf[12] = cea;
-	buf[13] = vesaceanr;
-	serversocket_write(12 + val, buf);
+	buf[CMDBUF_OFFSET] = cea;
+	buf[CMDBUF_OFFSET + 1] = vesaceanr;
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -962,14 +962,14 @@ int hdmi_service_fb_release(void)
 	__u8 buf[32];
 
 	val = HDMI_FB_RELEASE;
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = 0;
-	memcpy(&buf[8], &val, 4);
-	serversocket_write(12 + val, buf);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -984,19 +984,19 @@ int hdmi_service_cec_send(__u8 initiator, __u8 destination, __u8 data_size,
 		return -1;
 
 	val = HDMI_CECSEND;
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = data_size + 3;
-	memcpy(&buf[8], &val, 4);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
 	/* data */
-	buf[12] = initiator;
-	buf[13] = destination;
-	buf[14] = data_size;
-	memcpy(&buf[15], data, data_size);
-	serversocket_write(12 + val, buf);
+	buf[CMDBUF_OFFSET] = initiator;
+	buf[CMDBUF_OFFSET + 1] = destination;
+	buf[CMDBUF_OFFSET + 2] = data_size;
+	memcpy(&buf[CMDBUF_OFFSET + 3], data, data_size);
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -1010,16 +1010,16 @@ int hdmi_service_edid_request(__u8 block)
 		return -1;
 
 	val = HDMI_EDIDREQ;
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = 1;
-	memcpy(&buf[8], &val, 4);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
 	/* data */
-	buf[12] = block;
-	serversocket_write(12 + val, buf);
+	buf[CMDBUF_OFFSET] = block;
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -1033,16 +1033,16 @@ int hdmi_service_hdcp_init(__u16 aes_size, __u8 *aes_data)
 		return -1;
 
 	val = HDMI_HDCP_INIT;
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = aes_size;
-	memcpy(&buf[8], &val, 4);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
 	/* data */
-	memcpy(&buf[12], aes_data, aes_size);
-	serversocket_write(12 + val, buf);
+	memcpy(&buf[CMDBUF_OFFSET], aes_data, aes_size);
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -1057,20 +1057,20 @@ int hdmi_service_infoframe_send(__u8 type, __u8 version, __u8 crc,
 		return -1;
 
 	val = HDMI_INFOFR;
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = data_size + 4;
-	memcpy(&buf[8], &val, 4);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
 	/* data */
-	buf[12] = type;
-	buf[13] = version;
-	buf[14] = crc;
-	buf[15] = data_size;
-	memcpy(&buf[16], data, data_size);
-	serversocket_write(12 + val, buf);
+	buf[CMDBUF_OFFSET] = type;
+	buf[CMDBUF_OFFSET + 1] = version;
+	buf[CMDBUF_OFFSET + 2] = crc;
+	buf[CMDBUF_OFFSET + 3] = data_size;
+	memcpy(&buf[CMDBUF_OFFSET + 4], data, data_size);
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
@@ -1083,22 +1083,22 @@ int hdmi_service_vesa_cea_prio_set(__u8 vesa_cea1, __u8 nr1,
 	__u8 buf[32];
 
 	val = HDMI_VESACEAPRIO_SET;
-	memcpy(&buf[0], &val, 4);
+	memcpy(&buf[CMD_OFFSET], &val, 4);
 	/* cmd_id */
 	val = 0;
-	memcpy(&buf[4], &val, 4);
+	memcpy(&buf[CMDID_OFFSET], &val, 4);
 	/* len */
 	val = 7;
-	memcpy(&buf[8], &val, 4);
+	memcpy(&buf[CMDLEN_OFFSET], &val, 4);
 	/* data */
-	buf[12] = 3;
-	buf[13] = vesa_cea1;
-	buf[14] = nr1;
-	buf[15] = vesa_cea2;
-	buf[16] = nr2;
-	buf[17] = vesa_cea3;
-	buf[18] = nr3;
-	serversocket_write(12 + val, buf);
+	buf[CMDBUF_OFFSET] = 3;
+	buf[CMDBUF_OFFSET + 1] = vesa_cea1;
+	buf[CMDBUF_OFFSET + 2] = nr1;
+	buf[CMDBUF_OFFSET + 3] = vesa_cea2;
+	buf[CMDBUF_OFFSET + 4] = nr2;
+	buf[CMDBUF_OFFSET + 5] = vesa_cea3;
+	buf[CMDBUF_OFFSET + 6] = nr3;
+	serversocket_write(CMDBUF_OFFSET + val, buf);
 
 	return 0;
 }
diff --git a/src/setres.c b/src/setres.c
index 1bf566f..4e13c6e 100644
--- a/src/setres.c
+++ b/src/setres.c
@@ -46,7 +46,8 @@ int vesacea_supported(int *nr, struct vesacea vesacea[])
 		if (video_formats[index].sink_support) {
 			vesacea[*nr].cea = video_formats[index].cea;
 			vesacea[*nr].nr = video_formats[index].vesaceanr;
-			LOGHDMILIB2("cea:%d nr:%d", vesacea[*nr].cea, vesacea[*nr].nr);
+			LOGHDMILIB2("cea:%d nr:%d", vesacea[*nr].cea,
+							vesacea[*nr].nr);
 			(*nr)++;
 		}
 	}
@@ -174,31 +175,31 @@ static int vesaceanrtovar(struct fb_var_screeninfo *var, __u8 cea,
 	return -EINVAL;
 }
 
-static void vesacea_prio_default(void)
+void vesacea_prio_default(void)
 {
 	/* 1920x1080P@30 */
 	vesaceaprio[0].cea = 1;
 	vesaceaprio[0].nr = 34;
 
-	/* 1920x1080P@25 */
+	/* 1280x720P@60 */
 	vesaceaprio[1].cea = 1;
-	vesaceaprio[1].nr = 33;
+	vesaceaprio[1].nr = 4;
 
-	/* 1920x1080P@24 */
+	/* 1920x1080P@25 */
 	vesaceaprio[2].cea = 1;
-	vesaceaprio[2].nr = 32;
+	vesaceaprio[2].nr = 33;
 
-	/* 1920x1080I@60 */
+	/* 1920x1080P@24 */
 	vesaceaprio[3].cea = 1;
-	vesaceaprio[3].nr = 5;
+	vesaceaprio[3].nr = 32;
 
-	/* 1920x1080I@30 */
+	/* 1920x1080I@60 */
 	vesaceaprio[4].cea = 1;
-	vesaceaprio[4].nr = 20;
+	vesaceaprio[4].nr = 5;
 
-	/* 1280x720P@60 */
+	/* 1920x1080I@30 */
 	vesaceaprio[5].cea = 1;
-	vesaceaprio[5].nr = 4;
+	vesaceaprio[5].nr = 20;
 
 	/* 1280x720P@50 */
 	vesaceaprio[6].cea = 1;
@@ -232,8 +233,6 @@ void set_vesacea_prio_all(void)
 {
 	int index;
 
-	vesacea_prio_default();
-
 	/* Set cea prio. Continue until prio = 0 or maxsize */
 	for (index = 0; index < CEAPRIO_MAX_SIZE; index++) {
 		LOGHDMILIB("index:%d cea:%d prio:%d",
diff --git a/src/socket.c b/src/socket.c
index e6256c3..a677cdd 100644
--- a/src/socket.c
+++ b/src/socket.c
@@ -56,11 +56,12 @@ int clientsocket_get(void)
 /* Client socket thread. Handles incoming socket messages */
 static void thread_sockclient_fn(void *arg)
 {
-	int res;
+	int bytes = 0;
 	char buffer[SOCKET_DATA_MAX];
 	struct cmd_data cmd_data;
 	int cont = 1;
 	int sock;
+	int buf_index = 0;
 
 	LOGHDMILIB("%s begin", __func__);
 
@@ -69,21 +70,40 @@ static void thread_sockclient_fn(void *arg)
 	LOGHDMILIB("clisock:%d", sock);
 
 	while (cont) {
-		memset(buffer, 0, SOCKET_DATA_MAX);
-		res = read(sock, buffer, SOCKET_DATA_MAX);
-		if (res <= 0) {
-			LOGHDMILIB("clisocket closed:%d", res);
-			goto thread_sockclient_fn_end;
+		if (bytes < CMDBUF_OFFSET) {
+			/* Read from socket */
+			bytes += read(sock, buffer + buf_index,
+					SOCKET_DATA_MAX - buf_index);
+			if (bytes <= 0) {
+				LOGHDMILIB("clisocket closed:%d", bytes);
+				goto thread_sockclient_fn_end;
+			}
+
+			LOGHDMILIB("clisockread:%d", bytes);
+
+			if (bytes < CMDBUF_OFFSET)
+				/* Not enough data */
+				continue;
 		}
 
-		LOGHDMILIB("clisockread res:%d", res);
-
-		cmd_data.cmd = (__u32)buffer[0];
-		cmd_data.cmd_id = (__u32)buffer[4];
-		cmd_data.data_len = (__u32)buffer[8];
-		memcpy(cmd_data.data, &buffer[12], cmd_data.data_len);
+		/* Valid command */
+		cmd_data.cmd = (__u32)buffer[buf_index + CMD_OFFSET];
+		cmd_data.cmd_id = (__u32)buffer[buf_index + CMDID_OFFSET];
+		cmd_data.data_len = (__u32)buffer[buf_index + CMDLEN_OFFSET];
+		memcpy(cmd_data.data, &buffer[buf_index + CMDBUF_OFFSET],
+					cmd_data.data_len);
 		cmd_data.next = NULL;
 
+		/* Remaining bytes to handle */
+		bytes -= (CMDBUF_OFFSET + cmd_data.data_len);
+		buf_index += (CMDBUF_OFFSET + cmd_data.data_len);
+		if (bytes < 0)
+			bytes = 0;
+		if (bytes < CMDBUF_OFFSET) {
+			memcpy(buffer, buffer + buf_index, bytes);
+			buf_index = bytes;
+		}
+
 		/* Add to list */
 		cmd_add(&cmd_data);
 
@@ -98,7 +118,7 @@ thread_sockclient_fn_end:
 	close(sock);
 	clientsocket_set(-1);
 
-	LOGHDMILIB("%s end res:%d", __func__, res);
+	LOGHDMILIB("%s end: %d", __func__, bytes);
 	pthread_exit(NULL);
 }
 
@@ -226,10 +246,11 @@ static void thread_sockserver_fn(void *arg)
 		if (cmd_data.cmd == HDMI_EXIT) {
 			cont = 0;
 		} else {
-			cmd_data.cmd = (__u32)buffer[0];
-			cmd_data.cmd_id = (__u32)buffer[4];
-			cmd_data.data_len = (__u32)buffer[8];
-			memcpy(cmd_data.data, &buffer[12], cmd_data.data_len);
+			cmd_data.cmd = (__u32)buffer[CMD_OFFSET];
+			cmd_data.cmd_id = (__u32)buffer[CMDID_OFFSET];
+			cmd_data.data_len = (__u32)buffer[CMDLEN_OFFSET];
+			memcpy(cmd_data.data, &buffer[CMDBUF_OFFSET],
+						cmd_data.data_len);
 			cmd_data.next = NULL;
 
 			/* Send through callback fn */