device: Add protocol version 2 support

1 files changed, 83 insertions, 12 deletions

diff --git a/src/device.c b/src/device.c
index 1c8ec8c..bb12310 100644
--- a/src/device.c
+++ b/src/device.c
@@ -50,6 +50,8 @@ int next_device_id;
 
 enum mux_protocol {
 	MUX_PROTO_VERSION = 0,
+	MUX_PROTO_CONTROL = 1,
+	MUX_PROTO_SETUP = 2,
 	MUX_PROTO_TCP = IPPROTO_TCP,
 };
 
@@ -71,6 +73,9 @@ struct mux_header
 {
 	uint32_t protocol;
 	uint32_t length;
+	uint32_t magic;
+	uint16_t tx_seq;
+	uint16_t rx_seq;
 };
 
 struct version_header
@@ -115,6 +120,9 @@ struct mux_device
 	unsigned char *pktbuf;
 	uint32_t pktlen;
 	void *preflight_cb_data;
+	int version;
+	uint16_t rx_seq;
+	uint16_t tx_seq;
 };
 
 static struct collection device_list;
@@ -183,6 +191,9 @@ static int send_packet(struct mux_device *dev, enum mux_protocol proto, void *he
 		case MUX_PROTO_VERSION:
 			hdrlen = sizeof(struct version_header);
 			break;
+		case MUX_PROTO_SETUP:
+			hdrlen = 0;
+			break;
 		case MUX_PROTO_TCP:
 			hdrlen = sizeof(struct tcphdr);
 			break;
@@ -192,7 +203,9 @@ static int send_packet(struct mux_device *dev, enum mux_protocol proto, void *he
 	}
 	usbmuxd_log(LL_SPEW, "send_packet(%d, 0x%x, %p, %p, %d)", dev->id, proto, header, data, length);
 
-	int total = sizeof(struct mux_header) + hdrlen + length;
+	int mux_header_size = ((dev->version < 2) ? 8 : sizeof(struct mux_header));
+
+	int total = mux_header_size + hdrlen + length;
 
 	if(total > USB_MTU) {
 		usbmuxd_log(LL_ERROR, "Tried to send packet larger than USB MTU (hdr %d data %d total %d) to device %d", hdrlen, length, total, dev->id);
@@ -203,9 +216,19 @@ static int send_packet(struct mux_device *dev, enum mux_protocol proto, void *he
 	struct mux_header *mhdr = (struct mux_header *)buffer;
 	mhdr->protocol = htonl(proto);
 	mhdr->length = htonl(total);
-	memcpy(buffer + sizeof(struct mux_header), header, hdrlen);
+	if (dev->version >= 2) {
+		mhdr->magic = htonl(0xfeedface);
+		if (proto == MUX_PROTO_SETUP) {
+			dev->tx_seq = 0;
+			dev->rx_seq = 0xFFFF;
+		}
+		mhdr->tx_seq = htons(dev->tx_seq);
+		mhdr->rx_seq = htons(dev->rx_seq);
+		dev->tx_seq++;
+	}	
+	memcpy(buffer + mux_header_size, header, hdrlen);
 	if(data && length)
-		memcpy(buffer + sizeof(struct mux_header) + hdrlen, data, length);
+		memcpy(buffer + mux_header_size + hdrlen, data, length);
 
 	if((res = usb_send(dev->usbdev, buffer, total)) < 0) {
 		usbmuxd_log(LL_ERROR, "usb_send failed while sending packet (len %d) to device %d: %d", total, dev->id, res);
@@ -512,7 +535,7 @@ static void device_version_input(struct mux_device *dev, struct version_header *
 	}
 	vh->major = ntohl(vh->major);
 	vh->minor = ntohl(vh->minor);
-	if(vh->major != 1 || vh->minor != 0) {
+	if(vh->major != 2 && vh->major != 1) {
 		usbmuxd_log(LL_ERROR, "Device %d has unknown version %d.%d", dev->id, vh->major, vh->minor);
 		pthread_mutex_lock(&device_list_mutex);
 		collection_remove(&device_list, dev);
@@ -520,7 +543,13 @@ static void device_version_input(struct mux_device *dev, struct version_header *
 		free(dev);
 		return;
 	}
-	usbmuxd_log(LL_NOTICE, "Connected to v%d.%d device %d on location 0x%x with serial number %s", vh->major, vh->minor, dev->id, usb_get_location(dev->usbdev), usb_get_serial(dev->usbdev));
+	dev->version = vh->major;
+
+	if (dev->version >= 2) {
+		send_packet(dev, MUX_PROTO_SETUP, NULL, "\x07", 1);
+	}
+
+	usbmuxd_log(LL_NOTICE, "Connected to v%d.%d device %d on location 0x%x with serial number %s", dev->version, vh->minor, dev->id, usb_get_location(dev->usbdev), usb_get_serial(dev->usbdev));
 	dev->state = MUXDEV_ACTIVE;
 	collection_init(&dev->connections);
 	struct device_info info;
@@ -531,6 +560,38 @@ static void device_version_input(struct mux_device *dev, struct version_header *
 	preflight_worker_device_add(&info);
 }
 
+static void device_control_input(struct mux_device *dev, unsigned char *payload, uint32_t payload_length)
+{
+	if (payload_length > 0) {
+		switch (payload[0]) {
+		case 3:
+			if (payload_length > 1) {
+				char* buf = malloc(payload_length);
+				strncpy(buf, (char*)payload+1, payload_length-1);
+				buf[payload_length-1] = '\0';
+				usbmuxd_log(LL_ERROR, "%s: ERROR: %s", __func__, buf);
+				free(buf);
+			} else {
+				usbmuxd_log(LL_ERROR, "%s: Error occured, but empty error message", __func__);
+			}
+			break;
+		case 7:
+			if (payload_length > 1) {
+				char* buf = malloc(payload_length);
+				strncpy(buf, (char*)payload+1, payload_length-1);
+				buf[payload_length-1] = '\0';
+				usbmuxd_log(LL_INFO, "%s: %s", __func__, buf);
+				free(buf);
+			}
+			break;
+		default:
+			break;
+		}
+	} else {
+		usbmuxd_log(LL_WARNING, "%s: got a type 1 packet without payload", __func__);
+	}
+}
+
 /**
  * Handle an incoming TCP packet from the device.
  *
@@ -686,7 +747,7 @@ void device_data_input(struct usb_device *usbdev, unsigned char *buffer, uint32_
 	}
 
 	struct mux_header *mhdr = (struct mux_header *)buffer;
-
+	int mux_header_size = ((dev->version < 2) ? 8 : sizeof(struct mux_header));
 	if(ntohl(mhdr->length) != length) {
 		usbmuxd_log(LL_ERROR, "Incoming packet size mismatch (dev %d, expected %d, got %d)", dev->id, ntohl(mhdr->length), length);
 		return;
@@ -696,23 +757,32 @@ void device_data_input(struct usb_device *usbdev, unsigned char *buffer, uint32_
 	unsigned char *payload;
 	uint32_t payload_length;
 
+	if (dev->version >= 2) {
+		dev->rx_seq = ntohs(mhdr->rx_seq);
+	}
+
 	switch(ntohl(mhdr->protocol)) {
 		case MUX_PROTO_VERSION:
-			if(length < (sizeof(struct mux_header) + sizeof(struct version_header))) {
+			if(length < (mux_header_size + sizeof(struct version_header))) {
 				usbmuxd_log(LL_ERROR, "Incoming version packet is too small (%d)", length);
 				return;
 			}
-			device_version_input(dev, (struct version_header *)(mhdr+1));
+			device_version_input(dev, (struct version_header *)((char*)mhdr+mux_header_size));
+			break;
+		case MUX_PROTO_CONTROL:
+			payload = (unsigned char *)(mhdr+1);
+			payload_length = length - mux_header_size;
+			device_control_input(dev, payload, payload_length);
 			break;
 		case MUX_PROTO_TCP:
-			if(length < (sizeof(struct mux_header) + sizeof(struct tcphdr))) {
+			if(length < (mux_header_size + sizeof(struct tcphdr))) {
 				usbmuxd_log(LL_ERROR, "Incoming TCP packet is too small (%d)", length);
 				return;
 			}
-			th = (struct tcphdr *)(mhdr+1);
+			th = (struct tcphdr *)((char*)mhdr+mux_header_size);
 			payload = (unsigned char *)(th+1);
-			payload_length = length - sizeof(struct tcphdr) - sizeof(struct mux_header);
-			device_tcp_input(dev, (struct tcphdr *)(mhdr+1), payload, payload_length);
+			payload_length = length - sizeof(struct tcphdr) - mux_header_size;
+			device_tcp_input(dev, th, payload, payload_length);
 			break;
 		default:
 			usbmuxd_log(LL_ERROR, "Incoming packet for device %d has unknown protocol 0x%x)", dev->id, ntohl(mhdr->protocol));
@@ -736,6 +806,7 @@ int device_add(struct usb_device *usbdev)
 	dev->pktbuf = malloc(DEV_MRU);
 	dev->pktlen = 0;
 	dev->preflight_cb_data = NULL;
+	dev->version = 0;
 	struct version_header vh;
 	vh.major = htonl(1);
 	vh.minor = htonl(0);