From a4856768f21dcc2aa9c99647784297f59330370f Mon Sep 17 00:00:00 2001 From: Paul Kocialkowski Date: Tue, 4 Feb 2014 19:54:56 +0100 Subject: [PATCH] modem_if: Inject and intercept RFS I/O messages to perform open/read/close Signed-off-by: Paul Kocialkowski --- drivers/misc/modem_if/sipc4_io_device.c | 182 ++++++++++++++++++++++++++++++++ 1 file changed, 182 insertions(+) diff --git a/drivers/misc/modem_if/sipc4_io_device.c b/drivers/misc/modem_if/sipc4_io_device.c index 28f95f7..413bea1 100644 --- a/drivers/misc/modem_if/sipc4_io_device.c +++ b/drivers/misc/modem_if/sipc4_io_device.c @@ -48,6 +48,8 @@ static const char hdlc_end[1] = { HDLC_END }; static int rx_iodev_skb(struct sk_buff *skb); +static int rfs_craft_start(struct io_device *iod); + static ssize_t show_waketime(struct device *dev, struct device_attribute *attr, char *buf) { @@ -685,6 +687,10 @@ static int rx_iodev_skb(struct sk_buff *skb) return rx_multi_fmt_frame(skb); case IPC_RFS: + dev_kfree_skb_any(skb); + mif_err("%s: Dropping RFS frame", __func__); + rfs_craft_start(iod); + return 0; default: skb_queue_tail(&iod->sk_rx_q, skb); mif_debug("wake up wq of %s\n", iod->name); @@ -1215,6 +1221,174 @@ static long misc_ioctl(struct file *filp, unsigned int cmd, unsigned long arg) return 0; } +// Craft an internal SKB frame +static int rfs_craft_skb(struct sk_buff **skb_p, unsigned char command, void *data, int length) +{ + struct sk_buff *skb = NULL; + struct rfs_hdr rfs_header; + int frame_length; + static unsigned char id = 0x0B; + + frame_length = sizeof(rfs_header) + length; + + rfs_header.len = frame_length; + rfs_header.cmd = command; + rfs_header.id = id; + + skb = alloc_skb(frame_length, GFP_KERNEL); + if (!skb) { + mif_err("fail alloc skb (%d)\n", __LINE__); + return -ENOMEM; + } + + memcpy(skb_put(skb, sizeof(rfs_header)), &rfs_header, sizeof(rfs_header)); + memcpy(skb_put(skb, length), data, length); + + *skb_p = skb; + + id++; + + return 0; +} + +// This is to start the RFS exchange, called when the first legitimate RFS frame is received +static int rfs_craft_start(struct io_device *iod) +{ + struct sk_buff *skb = NULL; + unsigned char buffer[100] = { 0 }; + unsigned char *p = NULL; + char path[] = "../../data/radio/test"; + int mode, path_length = 0; + int length = 0; + + static int started = 0; + + if (started) + return 0; + + mif_err("%s: Crafting open\n", __func__); + + p = &buffer; + length = 0; + + mode = O_RDWR; + memcpy(p, &mode, sizeof(mode)); + p += sizeof(mode); + length += sizeof(mode); + + path_length = strlen(path); + memcpy(p, &path_length, sizeof(path_length)); + p += sizeof(path_length); + length += sizeof(path_length); + + memcpy(p, &path, path_length); + length += path_length; + + // IPC_RFS_OPEN_FILE + rfs_craft_skb(&skb, 0x11, &buffer, length); + if (skb != NULL) { + mif_err("%s: Adding SKB to queue\n", __func__); + skb_queue_head(&iod->sk_rx_q, skb); + wake_up(&iod->wq); + } + + started = 1; + + return 0; +} + +// This is called upon user-space response to our crafted messages +static int rfs_craft_write(struct io_device *iod, void *data, int size) +{ + struct sk_buff *skb = NULL; + unsigned char buffer[100] = { 0 }; + struct rfs_hdr *rfs_header; + unsigned char *p; + int length; + int read_length; + static int fd = -1; + int errno; + + rfs_header = (struct rfs_hdr *) data; + + switch (rfs_header->cmd) { + case 0x11: // IPC_RFS_OPEN_FILE + p = data + sizeof(struct rfs_hdr); + + fd = *((int *) p); + p += sizeof(fd); + errno = *((int *) p); + + mif_err("%s: Open response: fd=%d, errno=%d\n", __func__, fd, errno); + + if (fd < 0 || errno) + break; + + p = &buffer; + length = 0; + + memcpy(p, &fd, sizeof(fd)); + p += sizeof(fd); + length += sizeof(fd); + + read_length = 12; + memcpy(p, &read_length, sizeof(read_length)); + p += sizeof(read_length); + length += sizeof(read_length); + + // IPC_RFS_READ_FILE + rfs_craft_skb(&skb, 0x03, &buffer, length); + if (skb != NULL) { + mif_err("%s: Adding SKB to queue\n", __func__); + skb_queue_head(&iod->sk_rx_q, skb); + wake_up(&iod->wq); + } + + break; + case 0x03: // IPC_RFS_READ_FILE + p = data + sizeof(struct rfs_hdr); + + read_length = *((int *) p); + p += sizeof(read_length); + + // Unknown int, perhaps offset + p += sizeof(int); + + mif_err("%s: Read response: %d bytes read\n", __func__, read_length); + mif_print_data(p, read_length); + + p = &buffer; + length = 0; + + if (unlikely(fd < 0)) + break; + + memcpy(p, &fd, sizeof(fd)); + p += sizeof(fd); + length += sizeof(fd); + + // IPC_RFS_CLOSE_FILE + rfs_craft_skb(&skb, 0x06, &buffer, length); + if (skb != NULL) { + mif_err("%s: Adding SKB to queue\n", __func__); + skb_queue_head(&iod->sk_rx_q, skb); + wake_up(&iod->wq); + } + + fd = -1; + + break; + default: + p = data + sizeof(struct rfs_hdr); + + mif_err("%s: Rx RFS message with command 0x%x and size %d\n", __func__, rfs_header->cmd, size); + mif_print_data(p, size - sizeof(struct rfs_hdr)); + break; + } + + return 0; +} + static ssize_t misc_write(struct file *filp, const char __user *buf, size_t count, loff_t *ppos) { @@ -1278,6 +1452,14 @@ static ssize_t misc_write(struct file *filp, const char __user *buf, skb_put(skb, calc_padding_size(iod, ld, skb->len)); + if (iod->format == IPC_RFS) { + mif_err("%s: Intercepted RFS response", __func__); + rfs_craft_write(iod, skb->data + SIZE_OF_HDLC_START, skb->len - SIZE_OF_HDLC_START - SIZE_OF_HDLC_END); + + dev_kfree_skb_any(skb); + return count; + } + #if 0 if (iod->format == IPC_FMT) { mif_err("\n<%s> Tx HDLC FMT frame (len %d)\n", -- 1.8.5.3