diff options
| -rw-r--r-- | src/device.c | 95 |
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); |