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",
|