1 files changed, 59 insertions, 30 deletions

diff --git a/libusbmuxd/libusbmuxd.c b/libusbmuxd/libusbmuxd.c
index db8d1d4..f5e5d1b 100644
--- a/libusbmuxd/libusbmuxd.c
+++ b/libusbmuxd/libusbmuxd.c
@@ -46,6 +46,8 @@ static usbmuxd_event_cb_t event_cb = NULL;
 pthread_t devmon;
 static int listenfd = -1;
 
+static int use_tag = 0;
+
 /**
  * Finds a device info record by its handle.
  * if the record is not found, NULL is returned.
@@ -152,6 +154,53 @@ static int usbmuxd_get_result(int sfd, uint32_t tag, uint32_t * result)
 	return -EPROTO;
 }
 
+static int send_packet(int sfd, uint32_t message, uint32_t tag, void *payload, uint32_t payload_size)
+{
+	struct usbmuxd_header header;
+
+	header.length = sizeof(struct usbmuxd_header);
+	header.version = USBMUXD_PROTOCOL_VERSION;
+	header.message = message;
+	header.tag = tag;
+	if (payload && (payload_size > 0)) {
+		header.length += payload_size;
+	}
+	int sent = send_buf(sfd, &header, sizeof(header));
+	if (sent != sizeof(header)) {
+		fprintf(stderr, "%s: ERROR: could not send packet header\n", __func__);
+		return -1;
+	}
+	if (payload && (payload_size > 0)) {
+		sent += send_buf(sfd, payload, payload_size);
+	}
+	if (sent != (int)header.length) {
+		fprintf(stderr, "%s: ERROR: could not send whole packet\n", __func__);
+		close(sfd);
+		return -1;
+	}
+	return sent;
+}
+
+static int send_listen_packet(int sfd, uint32_t tag)
+{
+	return send_packet(sfd, MESSAGE_LISTEN, tag, NULL, 0);
+}
+
+static int send_connect_packet(int sfd, uint32_t tag, uint32_t device_id, uint16_t port)
+{
+	struct {
+		uint32_t device_id;
+		uint16_t port;
+		uint16_t reserved;
+	} conninfo;
+
+	conninfo.device_id = device_id;
+	conninfo.port = htons(port);
+	conninfo.reserved = 0;
+
+	return send_packet(sfd, MESSAGE_CONNECT, tag, &conninfo, sizeof(conninfo));
+
+}
 
 /**
  * Generates an event, i.e. calls the callback function.
@@ -181,12 +230,6 @@ static int usbmuxd_listen()
 {
 	int sfd;
 	uint32_t res = -1;
-	struct usbmuxd_listen_request req;
-
-	req.header.length = sizeof(struct usbmuxd_listen_request);
-	req.header.version = USBMUXD_PROTOCOL_VERSION;
-	req.header.message = MESSAGE_LISTEN;
-	req.header.tag = 2;
 
 	sfd = connect_usbmuxd_socket();
 	if (sfd < 0) {
@@ -203,12 +246,13 @@ static int usbmuxd_listen()
 		return sfd;
 	}
 
-	if (send_buf(sfd, &req, req.header.length) != (int)req.header.length) {
+	use_tag++;
+	if (send_listen_packet(sfd, use_tag) <= 0) {
 		fprintf(stderr, "%s: ERROR: could not send listen packet\n", __func__);
 		close(sfd);
 		return -1;
 	}
-	if (usbmuxd_get_result(sfd, req.header.tag, &res) && (res != 0)) {
+	if (usbmuxd_get_result(sfd, use_tag, &res) && (res != 0)) {
 		close(sfd);
 		fprintf(stderr, "%s: ERROR: did not get OK but %d\n", __func__, res);
 		return -1;
@@ -343,7 +387,6 @@ int usbmuxd_unsubscribe()
 
 int usbmuxd_get_device_list(usbmuxd_device_info_t **device_list)
 {
-	struct usbmuxd_listen_request s_req;
 	int sfd;
 	int listen_success = 0;
 	uint32_t res;
@@ -359,17 +402,11 @@ int usbmuxd_get_device_list(usbmuxd_device_info_t **device_list)
 		return sfd;
 	}
 
-	s_req.header.length = sizeof(struct usbmuxd_listen_request);
-	s_req.header.version = USBMUXD_PROTOCOL_VERSION;
-	s_req.header.message = MESSAGE_LISTEN;
-	s_req.header.tag = 2;
-
-	// send scan request packet
-	if (send_buf(sfd, &s_req, s_req.header.length) ==
-		(int) s_req.header.length) {
+	use_tag++;
+	if (send_listen_packet(sfd, use_tag) > 0) {
 		res = -1;
 		// get response
-		if (usbmuxd_get_result(sfd, s_req.header.tag, &res) && (res == 0)) {
+		if (usbmuxd_get_result(sfd, use_tag, &res) && (res == 0)) {
 			listen_success = 1;
 		} else {
 			close(sfd);
@@ -481,7 +518,6 @@ int usbmuxd_get_device_by_uuid(const char *uuid, usbmuxd_device_info_t *device)
 int usbmuxd_connect(const int handle, const unsigned short port)
 {
 	int sfd;
-	struct usbmuxd_connect_request c_req;
 	int connected = 0;
 	uint32_t res = -1;
 
@@ -492,20 +528,13 @@ int usbmuxd_connect(const int handle, const unsigned short port)
 		return sfd;
 	}
 
-	c_req.header.length = sizeof(c_req);
-	c_req.header.version = USBMUXD_PROTOCOL_VERSION;
-	c_req.header.message = MESSAGE_CONNECT;
-	c_req.header.tag = 3;
-	c_req.device_id = (uint32_t) handle;
-	c_req.port = htons(port);
-	c_req.reserved = 0;
-
-	if (send_buf(sfd, &c_req, sizeof(c_req)) < 0) {
-		perror("send");
+	use_tag++;
+	if (send_connect_packet(sfd, use_tag, (uint32_t)handle, (uint16_t)port) <= 0) {
+		fprintf(stderr, "%s: Error sending connect message!\n", __func__);
 	} else {
 		// read ACK
 		//fprintf(stderr, "%s: Reading connect result...\n", __func__);
-		if (usbmuxd_get_result(sfd, c_req.header.tag, &res)) {
+		if (usbmuxd_get_result(sfd, use_tag, &res)) {
 			if (res == 0) {
 				//fprintf(stderr, "%s: Connect success!\n", __func__);
 				connected = 1;