SamsungGalaxyBackdoor ยป 0001-modem_if-Inject-and-intercept-RFS-I-O-messages-to-pe.patch
| drivers/misc/modem_if/sipc4_io_device.c | ||
|---|---|---|
|
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)
|
||
|
{
|
||
| ... | ... | |
|
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);
|
||
| ... | ... | |
|
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)
|
||
|
{
|
||
| ... | ... | |
|
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",
|
||