From a4856768f21dcc2aa9c99647784297f59330370f Mon Sep 17 00:00:00 2001
From: Paul Kocialkowski <contact@paulk.fr>
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 <contact@paulk.fr>
---
 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

