path: root/drivers/misc/modem_if_u1/sipc4_io_device.c

/* /linux/drivers/misc/modem_if/modem_io_device.c
 *
 * Copyright (C) 2010 Google, Inc.
 * Copyright (C) 2010 Samsung Electronics.
 *
 * This software is licensed under the terms of the GNU General Public
 * License version 2, as published by the Free Software Foundation, and
 * may be copied, distributed, and modified under those terms.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 */

#include <linux/init.h>
#include <linux/sched.h>
#include <linux/fs.h>
#include <linux/poll.h>
#include <linux/irq.h>
#include <linux/gpio.h>
#include <linux/if_arp.h>
#include <linux/ip.h>
#include <linux/if_ether.h>
#include <linux/etherdevice.h>
#include <linux/device.h>

#include <linux/platform_data/modem.h>
#ifdef CONFIG_LINK_DEVICE_C2C
#include <linux/platform_data/c2c.h>
#endif
#include "modem_prj.h"
#include "modem_utils.h"

/*
 * MAX_RXDATA_SIZE is used at making skb, when it called with page size
 * it need more bytes to allocate itself (Ex, cache byte, shared info,
 * padding...)
 * So, give restriction to allocation size below 1 page to prevent
 * big pages broken.
 */
#define MAX_RXDATA_SIZE		0x0E00	/* 4 * 1024 - 512 */
#define MAX_BOOTDATA_SIZE	0x4008	/* EBL package format*/
#define MAX_MULTI_FMT_SIZE	0x4000	/* 16 * 1024 */

static const char hdlc_start[1] = { HDLC_START };
static const char hdlc_end[1] = { HDLC_END };

static int rx_iodev_skb(struct sk_buff *skb);

static ssize_t show_waketime(struct device *dev,
		struct device_attribute *attr, char *buf)
{
	unsigned int msec;
	char *p = buf;
	struct miscdevice *miscdev = dev_get_drvdata(dev);
	struct io_device *iod = container_of(miscdev, struct io_device,
			miscdev);

	msec = jiffies_to_msecs(iod->waketime);

	p += sprintf(buf, "raw waketime : %ums\n", msec);

	return p - buf;
}

static ssize_t store_waketime(struct device *dev,
		struct device_attribute *attr, const char *buf, size_t count)
{
	unsigned long msec;
	int ret;
	struct miscdevice *miscdev = dev_get_drvdata(dev);
	struct io_device *iod = container_of(miscdev, struct io_device,
			miscdev);

	ret = strict_strtoul(buf, 10, &msec);
	if (ret)
		return count;

	iod->waketime = msecs_to_jiffies(msec);

	return count;
}

static struct device_attribute attr_waketime =
	__ATTR(waketime, S_IRUGO | S_IWUSR, show_waketime, store_waketime);

static ssize_t show_loopback(struct device *dev,
		struct device_attribute *attr, char *buf)
{
	struct miscdevice *miscdev = dev_get_drvdata(dev);
	struct modem_shared *msd =
		container_of(miscdev, struct io_device, miscdev)->msd;
	unsigned char *ip = (unsigned char *)&msd->loopback_ipaddr;
	char *p = buf;

	p += sprintf(buf, "%u.%u.%u.%u\n", ip[0], ip[1], ip[2], ip[3]);

	return p - buf;
}

static ssize_t store_loopback(struct device *dev,
		struct device_attribute *attr, const char *buf, size_t count)
{
	struct miscdevice *miscdev = dev_get_drvdata(dev);
	struct modem_shared *msd =
		container_of(miscdev, struct io_device, miscdev)->msd;

	msd->loopback_ipaddr = ipv4str_to_be32(buf, count);

	return count;
}

static struct device_attribute attr_loopback =
	__ATTR(loopback, S_IRUGO | S_IWUSR, show_loopback, store_loopback);

static int get_header_size(struct io_device *iod)
{
	switch (iod->format) {
	case IPC_FMT:
		return sizeof(struct fmt_hdr);

	case IPC_RAW:
	case IPC_MULTI_RAW:
		return sizeof(struct raw_hdr);

	case IPC_RFS:
		return sizeof(struct rfs_hdr);

	case IPC_BOOT:
		/* minimum size for transaction align */
		return 4;

	case IPC_RAMDUMP:
	default:
		return 0;
	}
}

static int get_hdlc_size(struct io_device *iod, char *buf)
{
	struct fmt_hdr *fmt_header;
	struct raw_hdr *raw_header;
	struct rfs_hdr *rfs_header;

	mif_debug("buf : %02x %02x %02x (%d)\n", *buf, *(buf + 1),
				*(buf + 2), __LINE__);

	switch (iod->format) {
	case IPC_FMT:
		fmt_header = (struct fmt_hdr *)buf;
		if (iod->mc->mdm_data->ipc_version == SIPC_VER_42)
			return fmt_header->len & 0x3FFF;
		else
			return fmt_header->len;
	case IPC_RAW:
	case IPC_MULTI_RAW:
		raw_header = (struct raw_hdr *)buf;
		return raw_header->len;
	case IPC_RFS:
		rfs_header = (struct rfs_hdr *)buf;
		return rfs_header->len;
	default:
		break;
	}
	return 0;
}

static void *get_header(struct io_device *iod, size_t count,
			char *frame_header_buf)
{
	struct fmt_hdr *fmt_h;
	struct raw_hdr *raw_h;
	struct rfs_hdr *rfs_h;

	switch (iod->format) {
	case IPC_FMT:
		fmt_h = (struct fmt_hdr *)frame_header_buf;

		fmt_h->len = count + sizeof(struct fmt_hdr);
		fmt_h->control = 0;

		return (void *)frame_header_buf;

	case IPC_RAW:
	case IPC_MULTI_RAW:
		raw_h = (struct raw_hdr *)frame_header_buf;

		raw_h->len = count + sizeof(struct raw_hdr);
		raw_h->channel = iod->id & 0x1F;
		raw_h->control = 0;

		return (void *)frame_header_buf;

	case IPC_RFS:
		rfs_h = (struct rfs_hdr *)frame_header_buf;

		rfs_h->len = count + sizeof(struct raw_hdr);
		rfs_h->id = iod->id;

		return (void *)frame_header_buf;

	default:
		return 0;
	}
}

static inline int calc_padding_size(struct io_device *iod,
	struct link_device *ld, unsigned len)
{
	if (ld->aligned)
		return (4 - (len & 0x3)) & 0x3;
	else
		return 0;
}

static inline int rx_hdlc_head_start_check(char *buf)
{
	/* check hdlc head and return size of start byte */
	return (buf[0] == HDLC_START) ? SIZE_OF_HDLC_START : -EBADMSG;
}

static inline int rx_hdlc_tail_check(char *buf)
{
	/* check hdlc tail and return size of tail byte */
	return (buf[0] == HDLC_END) ? SIZE_OF_HDLC_END : -EBADMSG;
}

/* remove hdlc header and store IPC header */
static int rx_hdlc_head_check(struct io_device *iod, struct link_device *ld,
						char *buf, unsigned rest)
{
	struct header_data *hdr = &fragdata(iod, ld)->h_data;
	int head_size = get_header_size(iod);
	int done_len = 0;
	int len = 0;

	/* first frame, remove start header 7F */
	if (!hdr->start) {
		len = rx_hdlc_head_start_check(buf);
		if (len < 0) {
			mif_err("Wrong HDLC start: 0x%x\n", *buf);
			return len; /*Wrong hdlc start*/
		}

		mif_debug("check len : %d, rest : %d (%d)\n", len,
					rest, __LINE__);

		/* set the start flag of current packet */
		hdr->start = HDLC_START;
		hdr->len = 0;

		/* debug print */
		switch (iod->format) {
		case IPC_FMT:
		case IPC_RAW:
		case IPC_MULTI_RAW:
		case IPC_RFS:
			/* TODO: print buf...  */
			break;

		case IPC_CMD:
		case IPC_BOOT:
		case IPC_RAMDUMP:
		default:
			break;
		}

		buf += len;
		done_len += len;
		rest -= len; /* rest, call by value */
	}

	mif_debug("check len : %d, rest : %d (%d)\n",
			len, rest, __LINE__);

	/* store the HDLC header to iod priv */
	if (hdr->len < head_size) {
		len = min(rest, head_size - hdr->len);
		memcpy(hdr->hdr + hdr->len, buf, len);
		hdr->len += len;
		done_len += len;
	}

	mif_debug("check done_len : %d, rest : %d (%d)\n", done_len,
				rest, __LINE__);
	return done_len;
}

/* alloc skb and copy data to skb */
static int rx_hdlc_data_check(struct io_device *iod, struct link_device *ld,
						char *buf, unsigned rest)
{
	struct header_data *hdr = &fragdata(iod, ld)->h_data;
	struct sk_buff *skb = fragdata(iod, ld)->skb_recv;
	int head_size = get_header_size(iod);
	int data_size = get_hdlc_size(iod, hdr->hdr) - head_size;
	int alloc_size;
	int len = 0;
	int done_len = 0;
	int rest_len = data_size - hdr->frag_len;
	int continue_len = fragdata(iod, ld)->realloc_offset;

	mif_debug("head_size : %d, data_size : %d (%d)\n", head_size,
				data_size, __LINE__);

	if (continue_len) {
		/* check the HDLC header*/
		if (rx_hdlc_head_start_check(buf) == SIZE_OF_HDLC_START) {
			rest_len -= (head_size + SIZE_OF_HDLC_START);
			continue_len += (head_size + SIZE_OF_HDLC_START);
		}

		buf += continue_len;
		rest -= continue_len;
		done_len += continue_len;
		fragdata(iod, ld)->realloc_offset = 0;

		mif_debug("realloc_offset = %d\n", continue_len);
	}

	/* first payload data - alloc skb */
	if (!skb) {
		/* make skb data size under MAX_RXDATA_SIZE */
		alloc_size = min(data_size, MAX_RXDATA_SIZE);
		alloc_size = min(alloc_size, rest_len);

		/* exceptional case for RFS channel
		 * make skb for header info first
		 */
		if (iod->format == IPC_RFS && !hdr->frag_len) {
			skb = rx_alloc_skb(head_size, iod, ld);
			if (unlikely(!skb))
				return -ENOMEM;
			memcpy(skb_put(skb, head_size), hdr->hdr, head_size);
			rx_iodev_skb(skb);
		}

		/* allocate first packet for data, when its size exceed
		 * MAX_RXDATA_SIZE, this packet will split to
		 * multiple packets
		 */
		skb = rx_alloc_skb(alloc_size, iod, ld);
		if (unlikely(!skb)) {
			fragdata(iod, ld)->realloc_offset = continue_len;
			return -ENOMEM;
		}
		fragdata(iod, ld)->skb_recv = skb;
	}

	while (rest) {
		/* copy length cannot exceed rest_len */
		len = min_t(int, rest_len, rest);
		/* copy length should be under skb tailroom size */
		len = min(len, skb_tailroom(skb));
		/* when skb tailroom is bigger than MAX_RXDATA_SIZE
		 * restrict its size to MAX_RXDATA_SIZE just for convinience */
		len = min(len, MAX_RXDATA_SIZE);

		/* copy bytes to skb */
		memcpy(skb_put(skb, len), buf, len);

		/* adjusting variables */
		buf += len;
		rest -= len;
		done_len += len;
		rest_len -= len;
		hdr->frag_len += len;

		/* check if it is final for this packet sequence */
		if (!rest_len || !rest)
			break;

		/* more bytes are remain for this packet sequence
		 * pass fully loaded skb to rx queue
		 * and allocate another skb for continues data recv chain
		 */
		rx_iodev_skb(skb);
		fragdata(iod, ld)->skb_recv =  NULL;

		alloc_size = min(rest_len, MAX_RXDATA_SIZE);

		skb = rx_alloc_skb(alloc_size, iod, ld);
		if (unlikely(!skb)) {
			fragdata(iod, ld)->realloc_offset = done_len;
			return -ENOMEM;
		}
		fragdata(iod, ld)->skb_recv = skb;
	}

	mif_debug("rest : %d, alloc_size : %d , len : %d (%d)\n",
				rest, alloc_size, skb->len, __LINE__);

	return done_len;
}

static int rx_multi_fmt_frame(struct sk_buff *rx_skb)
{
	struct io_device *iod = skbpriv(rx_skb)->iod;
	struct link_device *ld = skbpriv(rx_skb)->ld;
	struct fmt_hdr *fh =
		(struct fmt_hdr *)fragdata(iod, ld)->h_data.hdr;
	unsigned int id = fh->control & 0x7F;
	struct sk_buff *skb = iod->skb[id];
	unsigned char *data = fragdata(iod, ld)->skb_recv->data;
	unsigned int rcvd = fragdata(iod, ld)->skb_recv->len;

	if (!skb) {
		/* If there has been no multiple frame with this ID */
		if (!(fh->control & 0x80)) {
			/* It is a single frame because the "more" bit is 0. */
#if 0
			mif_err("\n<%s> Rx FMT frame (len %d)\n",
				iod->name, rcvd);
			print_sipc4_fmt_frame(data);
			mif_err("\n");
#endif
			skb_queue_tail(&iod->sk_rx_q,
					fragdata(iod, ld)->skb_recv);
			mif_debug("wake up wq of %s\n", iod->name);
			wake_up(&iod->wq);
			return 0;
		} else {
			struct fmt_hdr *fh = NULL;
			skb = rx_alloc_skb(MAX_MULTI_FMT_SIZE, iod, ld);
			if (!skb) {
				mif_err("<%d> alloc_skb fail\n",
					__LINE__);
				return -ENOMEM;
			}
			iod->skb[id] = skb;

			fh = (struct fmt_hdr *)data;
			mif_info("Start multi-frame (ID %d, len %d)",
				id, fh->len);
		}
	}

	/* Start multi-frame processing */

	memcpy(skb_put(skb, rcvd), data, rcvd);
	dev_kfree_skb_any(fragdata(iod, ld)->skb_recv);

	if (fh->control & 0x80) {
		/* The last frame has not arrived yet. */
		mif_info("Receiving (ID %d, %d bytes)\n",
			id, skb->len);
	} else {
		/* It is the last frame because the "more" bit is 0. */
		mif_info("The Last (ID %d, %d bytes received)\n",
			id, skb->len);
#if 0
		mif_err("\n<%s> Rx FMT frame (len %d)\n",
			iod->name, skb->len);
		print_sipc4_fmt_frame(skb->data);
		mif_err("\n");
#endif
		skb_queue_tail(&iod->sk_rx_q, skb);
		iod->skb[id] = NULL;
		mif_info("wake up wq of %s\n", iod->name);
		wake_up(&iod->wq);
	}

	return 0;
}

static int rx_multi_fmt_frame_sipc42(struct sk_buff *rx_skb)
{
	struct io_device *iod = skbpriv(rx_skb)->iod;
	struct link_device *ld = skbpriv(rx_skb)->ld;
	struct fmt_hdr *fh =
		(struct fmt_hdr *)fragdata(iod, ld)->h_data.hdr;
	unsigned int    id = fh->control & 0x7F;
	struct sk_buff *skb = iod->skb[id];
	unsigned char  *data = fragdata(iod, ld)->skb_recv->data;
	unsigned int    rcvd = fragdata(iod, ld)->skb_recv->len;

	u8 ch;
	struct io_device *real_iod = NULL;

	ch = (fh->len & 0xC000) >> 14;
	fh->len = fh->len & 0x3FFF;
	real_iod = ld->fmt_iods[ch];
	if (!real_iod) {
		mif_err("wrong channel %d\n", ch);
		return -1;
	}
	skbpriv(rx_skb)->real_iod = real_iod;

	if (!skb) {
		/* If there has been no multiple frame with this ID */
		if (!(fh->control & 0x80)) {
			/* It is a single frame because the "more" bit is 0. */
#if 0
			mif_err("\n<%s> Rx FMT frame (len %d)\n",
				iod->name, rcvd);
			print_sipc4_fmt_frame(data);
			mif_err("\n");
#endif
			skb_queue_tail(&real_iod->sk_rx_q,
					fragdata(iod, ld)->skb_recv);
			mif_debug("wake up wq of %s\n", iod->name);
			wake_up(&real_iod->wq);
			return 0;
		} else {
			struct fmt_hdr *fh = NULL;
			skb = rx_alloc_skb(MAX_MULTI_FMT_SIZE, real_iod, ld);
			if (!skb) {
				mif_err("alloc_skb fail\n");
				return -ENOMEM;
			}
			real_iod->skb[id] = skb;

			fh = (struct fmt_hdr *)data;
			mif_err("Start multi-frame (ID %d, len %d)",
				id, fh->len);
		}
	}

	/* Start multi-frame processing */

	memcpy(skb_put(skb, rcvd), data, rcvd);
	dev_kfree_skb_any(fragdata(real_iod, ld)->skb_recv);

	if (fh->control & 0x80) {
		/* The last frame has not arrived yet. */
		mif_err("Receiving (ID %d, %d bytes)\n",
			id, skb->len);
	} else {
		/* It is the last frame because the "more" bit is 0. */
		mif_err("The Last (ID %d, %d bytes received)\n",
			id, skb->len);
#if 0
		mif_err("\n<%s> Rx FMT frame (len %d)\n",
			iod->name, skb->len);
		print_sipc4_fmt_frame(skb->data);
		mif_err("\n");
#endif
		skb_queue_tail(&real_iod->sk_rx_q, skb);
		real_iod->skb[id] = NULL;
		mif_info("wake up wq of %s\n", real_iod->name);
		wake_up(&real_iod->wq);
	}

	return 0;
}

static int rx_iodev_skb_raw(struct sk_buff *skb)
{
	int err = 0;
	struct io_device *iod = skbpriv(skb)->real_iod;
	struct net_device *ndev = NULL;
	struct iphdr *ip_header = NULL;
	struct ethhdr *ehdr = NULL;
	const char source[ETH_ALEN] = SOURCE_MAC_ADDR;

	/* check the real_iod is open? */
	/*
	if (atomic_read(&iod->opened) == 0) {
		mif_err("<%s> is not opened.\n",
			iod->name);
		pr_skb("drop packet", skb);
		return -ENOENT;
	}
	*/

	switch (iod->io_typ) {
	case IODEV_MISC:
		mif_debug("<%s> sk_rx_q.qlen = %d\n",
			iod->name, iod->sk_rx_q.qlen);
		skb_queue_tail(&iod->sk_rx_q, skb);
		wake_up(&iod->wq);
		return 0;

	case IODEV_NET:
		ndev = iod->ndev;
		if (!ndev) {
			mif_err("<%s> ndev == NULL",
				iod->name);
			return -EINVAL;
		}

		skb->dev = ndev;
		ndev->stats.rx_packets++;
		ndev->stats.rx_bytes += skb->len;

		/* check the version of IP */
		ip_header = (struct iphdr *)skb->data;
		if (ip_header->version == IP6VERSION)
			skb->protocol = htons(ETH_P_IPV6);
		else
			skb->protocol = htons(ETH_P_IP);

		if (iod->use_handover) {
			skb_push(skb, sizeof(struct ethhdr));
			ehdr = (void *)skb->data;
			memcpy(ehdr->h_dest, ndev->dev_addr, ETH_ALEN);
			memcpy(ehdr->h_source, source, ETH_ALEN);
			ehdr->h_proto = skb->protocol;
			skb->ip_summed = CHECKSUM_UNNECESSARY;
			skb_reset_mac_header(skb);

			skb_pull(skb, sizeof(struct ethhdr));
		}

		if (in_irq())
			err = netif_rx(skb);
		else
			err = netif_rx_ni(skb);

		if (err != NET_RX_SUCCESS)
			dev_err(&ndev->dev, "rx error: %d\n", err);

		return err;

	default:
		mif_err("wrong io_type : %d\n", iod->io_typ);
		return -EINVAL;
	}
}

static void rx_iodev_work(struct work_struct *work)
{
	int ret = 0;
	struct sk_buff *skb = NULL;
	struct io_device *iod = container_of(work, struct io_device,
				rx_work.work);

	while ((skb = skb_dequeue(&iod->sk_rx_q)) != NULL) {
		ret = rx_iodev_skb_raw(skb);
		if (ret < 0) {
			mif_err("<%s> rx_iodev_skb_raw err = %d",
				iod->name, ret);
			dev_kfree_skb_any(skb);
		} else if (ret == NET_RX_DROP) {
			mif_err("<%s> ret == NET_RX_DROP\n",
				iod->name);
			schedule_delayed_work(&iod->rx_work,
						msecs_to_jiffies(100));
			break;
		}
	}
}

static int rx_multipdp(struct sk_buff *skb)
{
	u8 ch;
	struct io_device *iod = skbpriv(skb)->iod;
	struct link_device *ld = skbpriv(skb)->ld;
	struct raw_hdr *raw_header =
		(struct raw_hdr *)fragdata(iod, ld)->h_data.hdr;
	struct io_device *real_iod = NULL;

	ch = raw_header->channel;
	if (ch == DATA_LOOPBACK_CHANNEL && ld->msd->loopback_ipaddr)
		ch = RMNET0_CH_ID;

	real_iod = link_get_iod_with_channel(ld, 0x20 | ch);
	if (!real_iod) {
		mif_err("wrong channel %d\n", ch);
		return -1;
	}

	skbpriv(skb)->real_iod = real_iod;
	skb_queue_tail(&iod->sk_rx_q, skb);
	mif_debug("sk_rx_qlen:%d\n", iod->sk_rx_q.qlen);

	schedule_delayed_work(&iod->rx_work, 0);
	return 0;
}

/* de-mux function draft */
static int rx_iodev_skb(struct sk_buff *skb)
{
	struct io_device *iod = skbpriv(skb)->iod;

	switch (iod->format) {
	case IPC_MULTI_RAW:
		return rx_multipdp(skb);

	case IPC_FMT:
		if (iod->mc->mdm_data->ipc_version == SIPC_VER_42)
			return rx_multi_fmt_frame_sipc42(skb);
		else
			return rx_multi_fmt_frame(skb);

	case IPC_RFS:
	default:
		skb_queue_tail(&iod->sk_rx_q, skb);
		mif_debug("wake up wq of %s\n", iod->name);
		wake_up(&iod->wq);
		return 0;
	}
}

static int rx_hdlc_packet(struct io_device *iod, struct link_device *ld,
		const char *data, unsigned recv_size)
{
	int rest = (int)recv_size;
	char *buf = (char *)data;
	int err = 0;
	int len = 0;
	unsigned rcvd = 0;

	if (rest <= 0)
		goto exit;

	mif_debug("RX_SIZE = %d, ld: %s\n", rest, ld->name);

	if (fragdata(iod, ld)->h_data.frag_len) {
		/*
		  If the fragdata(iod, ld)->h_data.frag_len field is
		  not zero, there is a HDLC frame that is waiting for more data
		  or HDLC_END in the skb (fragdata(iod, ld)->skb_recv).
		  In this case, rx_hdlc_head_check() must be skipped.
		*/
		goto data_check;
	}

next_frame:
	err = len = rx_hdlc_head_check(iod, ld, buf, rest);
	if (err < 0)
		goto exit;
	mif_debug("check len : %d, rest : %d (%d)\n", len, rest,
				__LINE__);

	buf += len;
	rest -= len;
	if (rest <= 0)
		goto exit;

data_check:
	/*
	  If the return value of rx_hdlc_data_check() is zero, there remains
	  only HDLC_END that will be received.
	*/
	err = len = rx_hdlc_data_check(iod, ld, buf, rest);
	if (err < 0)
		goto exit;
	mif_debug("check len : %d, rest : %d (%d)\n", len, rest,
				__LINE__);

	buf += len;
	rest -= len;

	if (!rest && fragdata(iod, ld)->h_data.frag_len) {
		/*
		  Data is being received and more data or HDLC_END does not
		  arrive yet, but there is no more data in the buffer. More
		  data may come within the next frame from the link device.
		*/
		return 0;
	} else if (rest <= 0)
		goto exit;

	/* At this point, one HDLC frame except HDLC_END has been received. */

	err = len = rx_hdlc_tail_check(buf);
	if (err < 0) {
		mif_err("Wrong HDLC end: 0x%02X\n", *buf);
		goto exit;
	}
	mif_debug("check len : %d, rest : %d (%d)\n", len, rest,
				__LINE__);
	buf += len;
	rest -= len;

	/* At this point, one complete HDLC frame has been received. */

	/*
	  The padding size is applied for the next HDLC frame. Zero will be
	  returned by calc_padding_size() if the link device does not require
	  4-byte aligned access.
	*/
	rcvd = get_hdlc_size(iod, fragdata(iod, ld)->h_data.hdr) +
	       (SIZE_OF_HDLC_START + SIZE_OF_HDLC_END);
	len = calc_padding_size(iod, ld, rcvd);
	buf += len;
	rest -= len;
	if (rest < 0)
		goto exit;

	err = rx_iodev_skb(fragdata(iod, ld)->skb_recv);
	if (err < 0)
		goto exit;

	/* initialize header & skb */
	fragdata(iod, ld)->skb_recv = NULL;
	memset(&fragdata(iod, ld)->h_data, 0x00,
			sizeof(struct header_data));
	fragdata(iod, ld)->realloc_offset = 0;

	if (rest)
		goto next_frame;

exit:
	/* free buffers. mipi-hsi re-use recv buf */

	if (rest < 0)
		err = -ERANGE;

	if (err == -ENOMEM) {
		if (!(fragdata(iod, ld)->h_data.frag_len))
			memset(&fragdata(iod, ld)->h_data, 0x00,
				sizeof(struct header_data));
		return err;
	}

	if (err < 0 && fragdata(iod, ld)->skb_recv) {
		dev_kfree_skb_any(fragdata(iod, ld)->skb_recv);
		fragdata(iod, ld)->skb_recv = NULL;

		/* clear headers */
		memset(&fragdata(iod, ld)->h_data, 0x00,
				sizeof(struct header_data));
		fragdata(iod, ld)->realloc_offset = 0;
	}

	return err;
}

static int rx_rfs_packet(struct io_device *iod, struct link_device *ld,
					const char *data, unsigned size)
{
	int err = 0;
	int pad = 0;
	int rcvd = 0;
	struct sk_buff *skb;

	if (data[0] != HDLC_START) {
		mif_err("Dropping RFS packet ... "
		       "size = %d, start = %02X %02X %02X %02X\n",
			size,
			data[0], data[1], data[2], data[3]);
		return -EINVAL;
	}

	if (data[size-1] != HDLC_END) {
		for (pad = 1; pad < 4; pad++)
			if (data[(size-1)-pad] == HDLC_END)
				break;

		if (pad >= 4) {
			char *b = (char *)data;
			unsigned sz = size;
			mif_err("size %d, No END_FLAG!!!\n", size);
			mif_err("end = %02X %02X %02X %02X\n",
				b[sz-4], b[sz-3], b[sz-2], b[sz-1]);
			return -EINVAL;
		} else {
			mif_info("padding = %d\n", pad);
		}
	}

	skb = rx_alloc_skb(size, iod, ld);
	if (unlikely(!skb)) {
		mif_err("alloc_skb fail\n");
		return -ENOMEM;
	}

	/* copy the RFS haeder to skb->data */
	rcvd = size - sizeof(hdlc_start) - sizeof(hdlc_end) - pad;
	memcpy(skb_put(skb, rcvd), ((char *)data + sizeof(hdlc_start)), rcvd);

	fragdata(iod, ld)->skb_recv = skb;
	err = rx_iodev_skb(fragdata(iod, ld)->skb_recv);

	return err;
}

/* called from link device when a packet arrives for this io device */
static int io_dev_recv_data_from_link_dev(struct io_device *iod,
		struct link_device *ld, const char *data, unsigned int len)
{
	struct sk_buff *skb;
	int err;
	unsigned int alloc_size, rest_len;
	char *cur;


	/* check the iod(except IODEV_DUMMY) is open?
	 * if the iod is MULTIPDP, check this data on rx_iodev_skb_raw()
	 * because, we cannot know the channel no in here.
	 */
	/*
	if (iod->io_typ != IODEV_DUMMY && atomic_read(&iod->opened) == 0) {
		mif_err("<%s> is not opened.\n", iod->name);
		pr_buffer("drop packet", data, len, 16u);
		return -ENOENT;
	}
	*/

	switch (iod->format) {
	case IPC_RFS:
#ifdef CONFIG_IPC_CMC22x_OLD_RFS
		err = rx_rfs_packet(iod, ld, data, len);
		return err;
#endif

	case IPC_FMT:
	case IPC_RAW:
	case IPC_MULTI_RAW:
		if (iod->waketime)
			wake_lock_timeout(&iod->wakelock, iod->waketime);
		err = rx_hdlc_packet(iod, ld, data, len);
		if (err < 0)
			mif_err("fail process HDLC frame\n");
		return err;

	case IPC_CMD:
		/* TODO- handle flow control command from CP */
		return 0;

	case IPC_BOOT:
	case IPC_RAMDUMP:
		/* save packet to sk_buff */
		skb = rx_alloc_skb(len, iod, ld);
		if (skb) {
			mif_debug("boot len : %d\n", len);

			memcpy(skb_put(skb, len), data, len);
			skb_queue_tail(&iod->sk_rx_q, skb);
			mif_debug("skb len : %d\n", skb->len);

			wake_up(&iod->wq);
			return len;
		}
		/* 32KB page alloc fail case, alloc 3.5K a page.. */
		mif_info("(%d)page fail, alloc fragment pages\n", len);

		rest_len = len;
		cur = (char *)data;
		while (rest_len) {
			alloc_size = min_t(unsigned int, MAX_RXDATA_SIZE,
				rest_len);
			skb = rx_alloc_skb(alloc_size, iod, ld);
			if (!skb) {
				mif_err("fail alloc skb (%d)\n", __LINE__);
				return -ENOMEM;
			}
			mif_debug("boot len : %d\n", alloc_size);

			memcpy(skb_put(skb, alloc_size), cur, alloc_size);
			skb_queue_tail(&iod->sk_rx_q, skb);
			mif_debug("skb len : %d\n", skb->len);

			rest_len -= alloc_size;
			cur += alloc_size;
		}
		wake_up(&iod->wq);
		return len;

	default:
		return -EINVAL;
	}
}

/* inform the IO device that the modem is now online or offline or
 * crashing or whatever...
 */
static void io_dev_modem_state_changed(struct io_device *iod,
			enum modem_state state)
{
	iod->mc->phone_state = state;
	mif_err("modem state changed. (iod: %s, state: %d)\n",
		iod->name, state);

	if ((state == STATE_CRASH_RESET) || (state == STATE_CRASH_EXIT)
		|| (state == STATE_NV_REBUILDING))
		wake_up(&iod->wq);
}

/**
 * io_dev_sim_state_changed
 * @iod:	IPC's io_device
 * @sim_online: SIM is online?
 */
static void io_dev_sim_state_changed(struct io_device *iod, bool sim_online)
{
	if (atomic_read(&iod->opened) == 0) {
		mif_err("iod is not opened: %s\n", iod->name);
		/* update latest sim status */
		iod->mc->sim_state.online = sim_online;
	} else if (iod->mc->sim_state.online == sim_online) {
		mif_err("sim state not changed.\n");
	} else {
		iod->mc->sim_state.online = sim_online;
		iod->mc->sim_state.changed = true;
		wake_lock_timeout(&iod->mc->bootd->wakelock,
						iod->mc->bootd->waketime);
		mif_err("sim state changed. (iod: %s, state: "
				"[online=%d, changed=%d])\n",
				iod->name, iod->mc->sim_state.online,
				iod->mc->sim_state.changed);
		wake_up(&iod->wq);
	}
}

static int misc_open(struct inode *inode, struct file *filp)
{
	struct io_device *iod = to_io_device(filp->private_data);
	struct modem_shared *msd = iod->msd;
	struct link_device *ld;
	int ret;
	filp->private_data = (void *)iod;

	mif_err("iod = %s\n", iod->name);
	atomic_inc(&iod->opened);

	list_for_each_entry(ld, &msd->link_dev_list, list) {
		if (IS_CONNECTED(iod, ld) && ld->init_comm) {
			ret = ld->init_comm(ld, iod);
			if (ret < 0) {
				mif_err("%s: init_comm error: %d\n",
						ld->name, ret);
				return ret;
			}
		}
	}

	return 0;
}

static int misc_release(struct inode *inode, struct file *filp)
{
	struct io_device *iod = (struct io_device *)filp->private_data;
	struct modem_shared *msd = iod->msd;
	struct link_device *ld;

	mif_err("iod = %s\n", iod->name);
	atomic_dec(&iod->opened);
	skb_queue_purge(&iod->sk_rx_q);

	list_for_each_entry(ld, &msd->link_dev_list, list) {
		if (IS_CONNECTED(iod, ld) && ld->terminate_comm)
			ld->terminate_comm(ld, iod);
	}

	return 0;
}

static unsigned int misc_poll(struct file *filp, struct poll_table_struct *wait)
{
	struct io_device *iod = (struct io_device *)filp->private_data;

	poll_wait(filp, &iod->wq, wait);

	if ((!skb_queue_empty(&iod->sk_rx_q)) &&
	    (iod->mc->phone_state != STATE_OFFLINE)) {
		return POLLIN | POLLRDNORM;
	} else if ((iod->mc->phone_state == STATE_CRASH_RESET) ||
			(iod->mc->phone_state == STATE_CRASH_EXIT) ||
			(iod->mc->phone_state == STATE_NV_REBUILDING) ||
#if defined(CONFIG_SEC_DUAL_MODEM_MODE)
			(iod->mc->phone_state == STATE_MODEM_SWITCH) ||
#endif
			(iod->mc->sim_state.changed)) {
		if (iod->format == IPC_RAW) {
			msleep(20);
			return 0;
		}
		return POLLHUP;
	} else {
		return 0;
	}
}

static long misc_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
{
	int p_state;
	struct io_device *iod = (struct io_device *)filp->private_data;
	struct link_device *ld = get_current_link(iod);
	char cpinfo_buf[530] = "CP Crash ";
	unsigned long size;
	int ret;
	char str[TASK_COMM_LEN];

	mif_debug("cmd = 0x%x\n", cmd);

	switch (cmd) {
	case IOCTL_MODEM_ON:
		mif_debug("misc_ioctl : IOCTL_MODEM_ON\n");
		return iod->mc->ops.modem_on(iod->mc);

	case IOCTL_MODEM_OFF:
		mif_debug("misc_ioctl : IOCTL_MODEM_OFF\n");
		return iod->mc->ops.modem_off(iod->mc);

	case IOCTL_MODEM_RESET:
		mif_debug("misc_ioctl : IOCTL_MODEM_RESET\n");
		return iod->mc->ops.modem_reset(iod->mc);

	case IOCTL_MODEM_BOOT_ON:
		mif_debug("misc_ioctl : IOCTL_MODEM_BOOT_ON\n");
		return iod->mc->ops.modem_boot_on(iod->mc);

	case IOCTL_MODEM_BOOT_OFF:
		mif_debug("misc_ioctl : IOCTL_MODEM_BOOT_OFF\n");
		return iod->mc->ops.modem_boot_off(iod->mc);

	/* TODO - will remove this command after ril updated */
	case IOCTL_MODEM_BOOT_DONE:
		mif_debug("misc_ioctl : IOCTL_MODEM_BOOT_DONE\n");
		return 0;

	case IOCTL_MODEM_STATUS:
		mif_debug("misc_ioctl : IOCTL_MODEM_STATUS\n");

		p_state = iod->mc->phone_state;
		if ((p_state == STATE_CRASH_RESET) ||
		    (p_state == STATE_CRASH_EXIT)) {
			mif_err("<%s> send err state : %d\n",
				iod->name, p_state);
		} else if (iod->mc->sim_state.changed &&
			!strcmp(get_task_comm(str, get_current()), "rild")) {
			int s_state = iod->mc->sim_state.online ?
					STATE_SIM_ATTACH : STATE_SIM_DETACH;
			iod->mc->sim_state.changed = false;

			mif_info("SIM states (%d) to %s\n", s_state, str);
			return s_state;
		} else if (p_state == STATE_NV_REBUILDING) {
			mif_info("send nv rebuild state : %d\n",
				p_state);
			iod->mc->phone_state = STATE_ONLINE;
		}
		return p_state;

	case IOCTL_MODEM_PROTOCOL_SUSPEND:
		mif_info("misc_ioctl : IOCTL_MODEM_PROTOCOL_SUSPEND\n");

		if (iod->format != IPC_MULTI_RAW)
			return -EINVAL;

		iodevs_for_each(iod->msd, iodev_netif_stop, 0);
		return 0;

	case IOCTL_MODEM_PROTOCOL_RESUME:
		mif_info("misc_ioctl : IOCTL_MODEM_PROTOCOL_RESUME\n");

		if (iod->format != IPC_MULTI_RAW)
			return -EINVAL;

		iodevs_for_each(iod->msd, iodev_netif_wake, 0);
		return 0;

	case IOCTL_MODEM_DUMP_START:
		mif_err("misc_ioctl : IOCTL_MODEM_DUMP_START\n");
		return ld->dump_start(ld, iod);

	case IOCTL_MODEM_DUMP_UPDATE:
		mif_debug("misc_ioctl : IOCTL_MODEM_DUMP_UPDATE\n");
		return ld->dump_update(ld, iod, arg);

	case IOCTL_MODEM_FORCE_CRASH_EXIT:
		mif_debug("misc_ioctl : IOCTL_MODEM_FORCE_CRASH_EXIT\n");
		if (iod->mc->ops.modem_force_crash_exit)
			return iod->mc->ops.modem_force_crash_exit(iod->mc);
		return -EINVAL;

	case IOCTL_MODEM_CP_UPLOAD:
		mif_err("misc_ioctl : IOCTL_MODEM_CP_UPLOAD\n");
		if (copy_from_user(cpinfo_buf + strlen(cpinfo_buf),
			(void __user *)arg, MAX_CPINFO_SIZE) != 0)
			panic("CP Crash");
		else
			panic(cpinfo_buf);
		return 0;

	case IOCTL_MODEM_DUMP_RESET:
		mif_err("misc_ioctl : IOCTL_MODEM_DUMP_RESET\n");
		return iod->mc->ops.modem_dump_reset(iod->mc);

#if defined(CONFIG_SEC_DUAL_MODEM_MODE)
	case IOCTL_MODEM_SWITCH_MODEM:
		mif_err("misc_ioctl : IOCTL_MODEM_SWITCH_MODEM\n");
		iod->mc->phone_state = STATE_MODEM_SWITCH;
		wake_up(&iod->wq);
		return 0;
#endif

	case IOCTL_MIF_LOG_DUMP:
		size = MAX_MIF_BUFF_SIZE;
		ret = copy_to_user((void __user *)arg, &size,
			sizeof(unsigned long));
		if (ret < 0)
			return -EFAULT;

		mif_dump_log(iod->mc->msd, iod);
		return 0;

	case IOCTL_MIF_DPRAM_DUMP:
#ifdef CONFIG_LINK_DEVICE_DPRAM
		if (iod->mc->mdm_data->link_types & LINKTYPE(LINKDEV_DPRAM)) {
			size = iod->mc->mdm_data->dpram_ctl->dp_size;
			ret = copy_to_user((void __user *)arg, &size,
				sizeof(unsigned long));
			if (ret < 0)
				return -EFAULT;
			mif_dump_dpram(iod);
			return 0;
		}
#endif
		return -EINVAL;

	default:
		 /* If you need to handle the ioctl for specific link device,
		  * then assign the link ioctl handler to ld->ioctl
		  * It will be call for specific link ioctl */
		if (ld->ioctl)
			return ld->ioctl(ld, iod, cmd, arg);

		mif_err("misc_ioctl : ioctl 0x%X is not defined.\n", cmd);
		return -EINVAL;
	}
	return 0;
}

static size_t _boot_write(struct io_device *iod, const char __user *buf,
								size_t count)
{
	int rest_len = count, frame_len = 0;
	char *cur = (char *)buf;
	struct sk_buff *skb = NULL;
	struct link_device *ld = get_current_link(iod);
	int ret;

	while (rest_len) {
		frame_len = min(rest_len, MAX_BOOTDATA_SIZE);
		skb = alloc_skb(frame_len, GFP_KERNEL);
		if (!skb) {
			mif_err("fail alloc skb (%d)\n", __LINE__);
			return -ENOMEM;
		}
		if (copy_from_user(
				skb_put(skb, frame_len), cur, frame_len) != 0) {
			dev_kfree_skb_any(skb);
			return -EFAULT;
		}
		rest_len -= frame_len;
		cur += frame_len;

		skbpriv(skb)->iod = iod;
		skbpriv(skb)->ld = ld;

		ret = ld->send(ld, iod, skb);
		if (ret < 0) {
			dev_kfree_skb_any(skb);
			return ret;
		}
	}
	return count;
}

static ssize_t misc_write(struct file *filp, const char __user *buf,
			size_t count, loff_t *ppos)
{
	struct io_device *iod = (struct io_device *)filp->private_data;
	struct link_device *ld = get_current_link(iod);
	int frame_len = 0;
	char frame_header_buf[sizeof(struct raw_hdr)];
	struct sk_buff *skb;
	int err;
	size_t tx_size;

	/* TODO - check here flow control for only raw data */

	frame_len = SIZE_OF_HDLC_START +
		    get_header_size(iod) +
		    count +
		    SIZE_OF_HDLC_END;
	if (ld->aligned)
		frame_len += MAX_LINK_PADDING_SIZE;

	skb = alloc_skb(frame_len, GFP_KERNEL);
	if (!skb) {
		if (frame_len > MAX_BOOTDATA_SIZE && iod->format == IPC_BOOT) {
			mif_info("large alloc fail\n");
			return _boot_write(iod, buf, count);
		}
		mif_err("fail alloc skb (%d)\n", __LINE__);
		return -ENOMEM;
	}

	switch (iod->format) {
	case IPC_BOOT:
	case IPC_RAMDUMP:
		if (copy_from_user(skb_put(skb, count), buf, count) != 0) {
			dev_kfree_skb_any(skb);
			return -EFAULT;
		}
		break;

	case IPC_RFS:
		memcpy(skb_put(skb, SIZE_OF_HDLC_START), hdlc_start,
				SIZE_OF_HDLC_START);
		if (copy_from_user(skb_put(skb, count), buf, count) != 0) {
			dev_kfree_skb_any(skb);
			return -EFAULT;
		}
		memcpy(skb_put(skb, SIZE_OF_HDLC_END), hdlc_end,
					SIZE_OF_HDLC_END);
		break;

	default:
		memcpy(skb_put(skb, SIZE_OF_HDLC_START), hdlc_start,
				SIZE_OF_HDLC_START);
		memcpy(skb_put(skb, get_header_size(iod)),
			get_header(iod, count, frame_header_buf),
			get_header_size(iod));
		if (copy_from_user(skb_put(skb, count), buf, count) != 0) {
			dev_kfree_skb_any(skb);
			return -EFAULT;
		}
		memcpy(skb_put(skb, SIZE_OF_HDLC_END), hdlc_end,
					SIZE_OF_HDLC_END);
		break;
	}

	skb_put(skb, calc_padding_size(iod, ld, skb->len));

#if 0
	if (iod->format == IPC_FMT) {
		mif_err("\n<%s> Tx HDLC FMT frame (len %d)\n",
			iod->name, skb->len);
		print_sipc4_hdlc_fmt_frame(skb->data);
		mif_err("\n");
	}
#endif
#if 0
	if (iod->format == IPC_RAW) {
		mif_err("\n<%s> Tx HDLC RAW frame (len %d)\n",
			iod->name, skb->len);
		mif_print_data(skb->data, (skb->len < 64 ? skb->len : 64));
		mif_err("\n");
	}
#endif
#if 0
	if (iod->format == IPC_RFS) {
		mif_err("\n<%s> Tx HDLC RFS frame (len %d)\n",
			iod->name, skb->len);
		mif_print_data(skb->data, (skb->len < 64 ? skb->len : 64));
		mif_err("\n");
	}
#endif

	/* send data with sk_buff, link device will put sk_buff
	 * into the specific sk_buff_q and run work-q to send data
	 */
	tx_size = skb->len;

	skbpriv(skb)->iod = iod;
	skbpriv(skb)->ld = ld;

	err = ld->send(ld, iod, skb);
	if (err < 0) {
		dev_kfree_skb_any(skb);
		return err;
	}

	if (err != tx_size)
		mif_err("WARNNING: wrong tx size: %s, format=%d "
			"count=%d, tx_size=%d, return_size=%d",
			iod->name, iod->format, count, tx_size, err);

	/* Temporaly enable t he RFS log for debugging IPC RX pedding issue */
	if (iod->format == IPC_RFS)
		mif_info("write rfs size = %d\n", count);

	return count;
}

static ssize_t misc_read(struct file *filp, char *buf, size_t count,
			loff_t *f_pos)
{
	struct io_device *iod = (struct io_device *)filp->private_data;
	struct sk_buff *skb = NULL;
	int pktsize = 0;
	unsigned int rest_len, copy_len;
	char *cur = buf;

	skb = skb_dequeue(&iod->sk_rx_q);
	if (!skb) {
		mif_err("<%s> no data from sk_rx_q\n", iod->name);
		return 0;
	}
	mif_debug("<%s> skb->len : %d\n", iod->name, skb->len);

	if (iod->format == IPC_BOOT) {
		pktsize = rest_len = count;
		while (rest_len) {
			if (skb->len > rest_len) {
				/* BOOT device receviced rx data as serial
				  stream, return data by User requested size */
				mif_err("skb->len %d > count %d\n", skb->len,
					rest_len);
				pr_skb("BOOT-wRX", skb);
				if (copy_to_user(cur, skb->data, rest_len)
									!= 0) {
					dev_kfree_skb_any(skb);
					return -EFAULT;
				}
				cur += rest_len;
				skb_pull(skb, rest_len);
				if (skb->len) {
					mif_info("queue-head, skb->len = %d\n",
						skb->len);
					skb_queue_head(&iod->sk_rx_q, skb);
				}
				mif_debug("return %u\n", rest_len);
				return rest_len;
			}

			copy_len = min(rest_len, skb->len);
			if (copy_to_user(cur, skb->data, copy_len) != 0) {
				dev_kfree_skb_any(skb);
				return -EFAULT;
			}
			cur += skb->len;
			dev_kfree_skb_any(skb);
			rest_len -= copy_len;

			if (!rest_len)
				break;

			skb = skb_dequeue(&iod->sk_rx_q);
			if (!skb) {
				mif_err("<%s> %d / %d sk_rx_q\n", iod->name,
					(count - rest_len), count);
				return count - rest_len;
			}
		}
	} else {
		if (skb->len > count) {
			mif_err("<%s> skb->len %d > count %d\n", iod->name,
				skb->len, count);
			dev_kfree_skb_any(skb);
			return -EFAULT;
		}
		pktsize = skb->len;
		if (copy_to_user(buf, skb->data, pktsize) != 0) {
			dev_kfree_skb_any(skb);
			return -EFAULT;
		}
		if (iod->format == IPC_FMT)
			mif_debug("copied %d bytes to user\n", pktsize);

		dev_kfree_skb_any(skb);
	}
	return pktsize;
}

#ifdef CONFIG_LINK_DEVICE_C2C
static int misc_mmap(struct file *filp, struct vm_area_struct *vma)
{
	int r = 0;
	unsigned long size = 0;
	unsigned long pfn = 0;
	unsigned long offset = 0;
	struct io_device *iod = (struct io_device *)filp->private_data;

	if (!vma)
		return -EFAULT;

	size = vma->vm_end - vma->vm_start;
	offset = vma->vm_pgoff << PAGE_SHIFT;
	if (offset + size > (C2C_CP_RGN_SIZE + C2C_SH_RGN_SIZE)) {
		mif_err("offset + size > C2C_CP_RGN_SIZE\n");
		return -EINVAL;
	}

	/* Set the noncacheable property to the region */
	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
	vma->vm_flags |= VM_RESERVED | VM_IO;

	pfn = __phys_to_pfn(C2C_CP_RGN_ADDR + offset);
	r = remap_pfn_range(vma, vma->vm_start, pfn, size, vma->vm_page_prot);
	if (r) {
		mif_err("Failed in remap_pfn_range()!!!\n");
		return -EAGAIN;
	}

	mif_err("VA = 0x%08lx, offset = 0x%lx, size = %lu\n",
		vma->vm_start, offset, size);

	return 0;
}
#endif

static const struct file_operations misc_io_fops = {
	.owner = THIS_MODULE,
	.open = misc_open,
	.release = misc_release,
	.poll = misc_poll,
	.unlocked_ioctl = misc_ioctl,
	.write = misc_write,
	.read = misc_read,
#ifdef CONFIG_LINK_DEVICE_C2C
	.mmap = misc_mmap,
#endif
};

static int vnet_open(struct net_device *ndev)
{
	struct vnet *vnet = netdev_priv(ndev);
	netif_start_queue(ndev);
	atomic_inc(&vnet->iod->opened);
	return 0;
}

static int vnet_stop(struct net_device *ndev)
{
	struct vnet *vnet = netdev_priv(ndev);
	atomic_dec(&vnet->iod->opened);
	netif_stop_queue(ndev);
	return 0;
}

static int vnet_xmit(struct sk_buff *skb, struct net_device *ndev)
{
	int ret = 0;
	int headroom = 0;
	int tailroom = 0;
	struct sk_buff *skb_new = NULL;
	struct vnet *vnet = netdev_priv(ndev);
	struct io_device *iod = vnet->iod;
	struct link_device *ld = get_current_link(iod);
	struct raw_hdr hd;
	struct iphdr *ip_header = NULL;

	/* When use `handover' with Network Bridge,
	 * user -> TCP/IP(kernel) -> bridge device -> TCP/IP(kernel) -> this.
	 *
	 * We remove the one ethernet header of skb before using skb->len,
	 * because the skb has two ethernet headers.
	 */
	if (iod->use_handover) {
		if (iod->id >= PSD_DATA_CHID_BEGIN &&
			iod->id <= PSD_DATA_CHID_END)
			skb_pull(skb, sizeof(struct ethhdr));
	}

	/* ip loop-back */
	ip_header = (struct iphdr *)skb->data;
	if (iod->msd->loopback_ipaddr &&
		ip_header->daddr == iod->msd->loopback_ipaddr) {
		swap(ip_header->saddr, ip_header->daddr);
		hd.channel = DATA_LOOPBACK_CHANNEL;
	} else {
		hd.channel = iod->id & 0x1F;
	}
	hd.len = skb->len + sizeof(hd);
	hd.control = 0;

	headroom = sizeof(hd) + sizeof(hdlc_start);
	tailroom = sizeof(hdlc_end);
	if (ld->aligned)
		tailroom += MAX_LINK_PADDING_SIZE;
	if (skb_headroom(skb) < headroom || skb_tailroom(skb) < tailroom) {
		skb_new = skb_copy_expand(skb, headroom, tailroom, GFP_ATOMIC);
		/* skb_copy_expand success or not, free old skb from caller */
		dev_kfree_skb_any(skb);
		if (!skb_new)
			return -ENOMEM;
	} else
		skb_new = skb;

	memcpy(skb_push(skb_new, sizeof(hd)), &hd, sizeof(hd));
	memcpy(skb_push(skb_new, sizeof(hdlc_start)), hdlc_start,
				sizeof(hdlc_start));
	memcpy(skb_put(skb_new, sizeof(hdlc_end)), hdlc_end, sizeof(hdlc_end));
	skb_put(skb_new, calc_padding_size(iod, ld,  skb_new->len));

	skbpriv(skb_new)->iod = iod;
	skbpriv(skb_new)->ld = ld;

	ret = ld->send(ld, iod, skb_new);
	if (ret < 0) {
		netif_stop_queue(ndev);
		dev_kfree_skb_any(skb_new);
		return NETDEV_TX_BUSY;
	}

	ndev->stats.tx_packets++;
	ndev->stats.tx_bytes += skb->len;

	return NETDEV_TX_OK;
}

static struct net_device_ops vnet_ops = {
	.ndo_open = vnet_open,
	.ndo_stop = vnet_stop,
	.ndo_start_xmit = vnet_xmit,
};

static void vnet_setup(struct net_device *ndev)
{
	ndev->netdev_ops = &vnet_ops;
	ndev->type = ARPHRD_PPP;
	ndev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
	ndev->addr_len = 0;
	ndev->hard_header_len = 0;
	ndev->tx_queue_len = 1000;
	ndev->mtu = ETH_DATA_LEN;
	ndev->watchdog_timeo = 5 * HZ;
}

static void vnet_setup_ether(struct net_device *ndev)
{
	ndev->netdev_ops = &vnet_ops;
	ndev->type = ARPHRD_ETHER;
	ndev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST | IFF_SLAVE;
	ndev->addr_len = ETH_ALEN;
	random_ether_addr(ndev->dev_addr);
	ndev->hard_header_len = 0;
	ndev->tx_queue_len = 1000;
	ndev->mtu = ETH_DATA_LEN;
	ndev->watchdog_timeo = 5 * HZ;
}

int sipc4_init_io_device(struct io_device *iod)
{
	int ret = 0;
	struct vnet *vnet;

	/* Get modem state from modem control device */
	iod->modem_state_changed = io_dev_modem_state_changed;

	iod->sim_state_changed = io_dev_sim_state_changed;

	/* Get data from link device */
	iod->recv = io_dev_recv_data_from_link_dev;

	/* Register misc or net device */
	switch (iod->io_typ) {
	case IODEV_MISC:
		init_waitqueue_head(&iod->wq);
		skb_queue_head_init(&iod->sk_rx_q);
		INIT_DELAYED_WORK(&iod->rx_work, rx_iodev_work);

		iod->miscdev.minor = MISC_DYNAMIC_MINOR;
		iod->miscdev.name = iod->name;
		iod->miscdev.fops = &misc_io_fops;

		ret = misc_register(&iod->miscdev);
		if (ret)
			mif_err("failed to register misc io device : %s\n",
						iod->name);

		break;

	case IODEV_NET:
		skb_queue_head_init(&iod->sk_rx_q);
		if (iod->use_handover)
			iod->ndev = alloc_netdev(0, iod->name,
						vnet_setup_ether);
		else
			iod->ndev = alloc_netdev(0, iod->name, vnet_setup);

		if (!iod->ndev) {
			mif_err("failed to alloc netdev\n");
			return -ENOMEM;
		}

		ret = register_netdev(iod->ndev);
		if (ret)
			free_netdev(iod->ndev);

		mif_debug("(iod:0x%p)\n", iod);
		vnet = netdev_priv(iod->ndev);
		mif_debug("(vnet:0x%p)\n", vnet);
		vnet->iod = iod;

		break;

	case IODEV_DUMMY:
		skb_queue_head_init(&iod->sk_rx_q);
		INIT_DELAYED_WORK(&iod->rx_work, rx_iodev_work);

		iod->miscdev.minor = MISC_DYNAMIC_MINOR;
		iod->miscdev.name = iod->name;
		iod->miscdev.fops = &misc_io_fops;

		ret = misc_register(&iod->miscdev);
		if (ret)
			mif_err("failed to register misc io device : %s\n",
						iod->name);
		ret = device_create_file(iod->miscdev.this_device,
				&attr_waketime);
		if (ret)
			mif_err("failed to create `waketime' file : %s\n",
					iod->name);
		ret = device_create_file(iod->miscdev.this_device,
				&attr_loopback);
		if (ret)
			mif_err("failed to create `loopback file' : %s\n",
					iod->name);
		break;

	default:
		mif_err("wrong io_type : %d\n", iod->io_typ);
		return -EINVAL;
	}

	mif_debug("%s(%d) : init_io_device() done : %d\n",
				iod->name, iod->io_typ, ret);
	return ret;
}