diff options
Diffstat (limited to 'drivers/misc/modem_if_u1/sipc4_io_device.c')
| -rw-r--r-- | drivers/misc/modem_if_u1/sipc4_io_device.c | 1710 |
1 files changed, 0 insertions, 1710 deletions
diff --git a/drivers/misc/modem_if_u1/sipc4_io_device.c b/drivers/misc/modem_if_u1/sipc4_io_device.c deleted file mode 100644 index 7330c28..0000000 --- a/drivers/misc/modem_if_u1/sipc4_io_device.c +++ /dev/null @@ -1,1710 +0,0 @@ -/* /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; -} - |