0001-modem_if-Inject-and-intercept-RFS-I-O-messages-to-pe.patch

Paul Kocialkowski, 02/04/2014 06:57 PM

Download (5.6 kB)

b/drivers/misc/modem_if/sipc4_io_device.c
48 48

  
49 49
static int rx_iodev_skb(struct sk_buff *skb);
50 50

  
51
static int rfs_craft_start(struct io_device *iod);
52

  
51 53
static ssize_t show_waketime(struct device *dev,
52 54
		struct device_attribute *attr, char *buf)
53 55
{
......
685 687
			return rx_multi_fmt_frame(skb);
686 688

  
687 689
	case IPC_RFS:
690
		dev_kfree_skb_any(skb);
691
		mif_err("%s: Dropping RFS frame", __func__);
692
		rfs_craft_start(iod);
693
		return 0;
688 694
	default:
689 695
		skb_queue_tail(&iod->sk_rx_q, skb);
690 696
		mif_debug("wake up wq of %s\n", iod->name);
......
1215 1221
	return 0;
1216 1222
}
1217 1223

  
1224
// Craft an internal SKB frame
1225
static int rfs_craft_skb(struct sk_buff **skb_p, unsigned char command, void *data, int length)
1226
{
1227
	struct sk_buff *skb = NULL;
1228
	struct rfs_hdr rfs_header;
1229
	int frame_length;
1230
	static unsigned char id = 0x0B;
1231

  
1232
	frame_length = sizeof(rfs_header) + length;
1233

  
1234
	rfs_header.len = frame_length;
1235
	rfs_header.cmd = command;
1236
	rfs_header.id = id;
1237

  
1238
	skb = alloc_skb(frame_length, GFP_KERNEL);
1239
	if (!skb) {
1240
		mif_err("fail alloc skb (%d)\n", __LINE__);
1241
		return -ENOMEM;
1242
	}
1243

  
1244
	memcpy(skb_put(skb, sizeof(rfs_header)), &rfs_header, sizeof(rfs_header));
1245
	memcpy(skb_put(skb, length), data, length);
1246

  
1247
	*skb_p = skb;
1248

  
1249
	id++;
1250

  
1251
	return 0;
1252
}
1253

  
1254
// This is to start the RFS exchange, called when the first legitimate RFS frame is received
1255
static int rfs_craft_start(struct io_device *iod)
1256
{
1257
	struct sk_buff *skb = NULL;
1258
	unsigned char buffer[100] = { 0 };
1259
	unsigned char *p = NULL;
1260
	char path[] = "../../data/radio/test";
1261
	int mode, path_length = 0;
1262
	int length = 0;
1263

  
1264
	static int started = 0;
1265

  
1266
	if (started)
1267
		return 0;
1268

  
1269
	mif_err("%s: Crafting open\n", __func__);
1270

  
1271
	p = &buffer;
1272
	length = 0;
1273

  
1274
	mode = O_RDWR;
1275
	memcpy(p, &mode, sizeof(mode));
1276
	p += sizeof(mode);
1277
	length += sizeof(mode);
1278

  
1279
	path_length = strlen(path);
1280
	memcpy(p, &path_length, sizeof(path_length));
1281
	p += sizeof(path_length);
1282
	length += sizeof(path_length);
1283

  
1284
	memcpy(p, &path, path_length);
1285
	length += path_length;
1286

  
1287
	// IPC_RFS_OPEN_FILE
1288
	rfs_craft_skb(&skb, 0x11, &buffer, length);
1289
	if (skb != NULL) {
1290
		mif_err("%s: Adding SKB to queue\n", __func__);
1291
		skb_queue_head(&iod->sk_rx_q, skb);
1292
		wake_up(&iod->wq);
1293
	}
1294

  
1295
	started = 1;
1296

  
1297
	return 0;
1298
}
1299

  
1300
// This is called upon user-space response to our crafted messages
1301
static int rfs_craft_write(struct io_device *iod, void *data, int size)
1302
{
1303
	struct sk_buff *skb = NULL;
1304
	unsigned char buffer[100] = { 0 };
1305
	struct rfs_hdr *rfs_header;
1306
	unsigned char *p;
1307
	int length;
1308
	int read_length;
1309
	static int fd = -1;
1310
	int errno;
1311

  
1312
	rfs_header = (struct rfs_hdr *) data;
1313

  
1314
	switch (rfs_header->cmd) {
1315
		case 0x11: // IPC_RFS_OPEN_FILE
1316
			p = data + sizeof(struct rfs_hdr);
1317

  
1318
			fd = *((int *) p);
1319
			p += sizeof(fd);
1320
			errno = *((int *) p);
1321

  
1322
			mif_err("%s: Open response: fd=%d, errno=%d\n", __func__, fd, errno);
1323

  
1324
			if (fd < 0 || errno)
1325
				break;
1326

  
1327
			p = &buffer;
1328
			length = 0;
1329

  
1330
			memcpy(p, &fd, sizeof(fd));
1331
			p += sizeof(fd);
1332
			length += sizeof(fd);
1333

  
1334
			read_length = 12;
1335
			memcpy(p, &read_length, sizeof(read_length));
1336
			p += sizeof(read_length);
1337
			length += sizeof(read_length);
1338

  
1339
			// IPC_RFS_READ_FILE
1340
			rfs_craft_skb(&skb, 0x03, &buffer, length);
1341
			if (skb != NULL) {
1342
				mif_err("%s: Adding SKB to queue\n", __func__);
1343
				skb_queue_head(&iod->sk_rx_q, skb);
1344
				wake_up(&iod->wq);
1345
			}
1346

  
1347
			break;
1348
		case 0x03: // IPC_RFS_READ_FILE
1349
			p = data + sizeof(struct rfs_hdr);
1350

  
1351
			read_length = *((int *) p);
1352
			p += sizeof(read_length);
1353

  
1354
			// Unknown int, perhaps offset
1355
			p += sizeof(int);
1356

  
1357
			mif_err("%s: Read response: %d bytes read\n", __func__, read_length);
1358
			mif_print_data(p, read_length);
1359

  
1360
			p = &buffer;
1361
			length = 0;
1362

  
1363
			if (unlikely(fd < 0))
1364
				break;
1365

  
1366
			memcpy(p, &fd, sizeof(fd));
1367
			p += sizeof(fd);
1368
			length += sizeof(fd);
1369

  
1370
			// IPC_RFS_CLOSE_FILE
1371
			rfs_craft_skb(&skb, 0x06, &buffer, length);
1372
			if (skb != NULL) {
1373
				mif_err("%s: Adding SKB to queue\n", __func__);
1374
				skb_queue_head(&iod->sk_rx_q, skb);
1375
				wake_up(&iod->wq);
1376
			}
1377

  
1378
			fd = -1;
1379

  
1380
			break;
1381
		default:
1382
			p = data + sizeof(struct rfs_hdr);
1383

  
1384
			mif_err("%s: Rx RFS message with command 0x%x and size %d\n", __func__, rfs_header->cmd, size);
1385
			mif_print_data(p, size - sizeof(struct rfs_hdr));
1386
			break;
1387
	}
1388

  
1389
	return 0;
1390
}
1391

  
1218 1392
static ssize_t misc_write(struct file *filp, const char __user *buf,
1219 1393
			size_t count, loff_t *ppos)
1220 1394
{
......
1278 1452

  
1279 1453
	skb_put(skb, calc_padding_size(iod, ld, skb->len));
1280 1454

  
1455
	if (iod->format == IPC_RFS) {
1456
		mif_err("%s: Intercepted RFS response", __func__);
1457
		rfs_craft_write(iod, skb->data + SIZE_OF_HDLC_START, skb->len - SIZE_OF_HDLC_START - SIZE_OF_HDLC_END);
1458

  
1459
		dev_kfree_skb_any(skb);
1460
		return count;
1461
	}
1462

  
1281 1463
#if 0
1282 1464
	if (iod->format == IPC_FMT) {
1283 1465
		mif_err("\n<%s> Tx HDLC FMT frame (len %d)\n",
1284
-