static int rx_submit(struct usbpn_dev *pnd, struct urb *req, gfp_t gfp_flags)
{
struct net_device *dev = pnd->dev;
struct page *page;
int err;
page = __netdev_alloc_page(dev, gfp_flags);
if (!page)
return -ENOMEM;
usb_fill_bulk_urb(req, pnd->usb, pnd->rx_pipe, page_address(page),
PAGE_SIZE, rx_complete, dev);
req->transfer_flags = 0;
err = usb_submit_urb(req, gfp_flags);
if (unlikely(err)) {
dev_dbg(&dev->dev, "RX submit error (%d)\n", err);
netdev_free_page(dev, page);
}
return err;
}
static int rx_submit(struct usbsvn *svn, int dev_id, struct urb *req,
gfp_t gfp_flags)
{
struct net_device *dev = svn->netdev;
struct usbsvn_devdata *devdata = &svn->devdata[dev_id];
struct usbsvn_rx *svn_rx;
struct page *page;
int err;
svn_rx = kzalloc(sizeof(struct usbsvn_rx), gfp_flags);
if (!svn_rx)
return -ENOMEM;
page = __netdev_alloc_page(dev, gfp_flags);
if (!page) {
kfree(svn_rx);
return -ENOMEM;
}
svn_rx->netdev = dev;
svn_rx->dev_id = dev_id;
usb_fill_bulk_urb(req, svn->usbdev, devdata->rx_pipe,
page_address(page), PAGE_SIZE, rx_complete, svn_rx);
req->transfer_flags = 0;
err = usb_submit_urb(req, gfp_flags);
if (unlikely(err)) {
dev_err(&dev->dev, "RX submit error (%d)\n", err);
kfree(svn_rx);
netdev_free_page(dev, page);
}
usb_mark_last_busy(req->dev);
return err;
}
static int sipc4_hdlc_rx(struct sipc4_rx_data *data)
{
int rest = data->size;
char *buf = page_address(data->page);
int len;
int err = -ERANGE;
if (rest <= 0)
goto end;
if (data->format == SIPC4_FMT)
printk(KERN_DEBUG "IPC:RX size=%d\n", data->size);
next_frame:
err = len = sipc4_check_header(data, buf, rest);
if (err < 0)
goto end;
buf += len;
rest -= len;
if (rest <= 0)
goto end;
err = len = sipc4_check_data(data, buf, rest);
if (err < 0)
goto end;
buf += len;
rest -= len;
if (rest <= 0)
goto end;
err = len = sipc4_check_hdlc_end(data, buf);
if (err < 0)
goto end;
buf += len;
rest -= len;
if (rest < 0)
goto end;
err = sipc4_hdlc_format_rx(data);
if (err < 0)
goto end;
memset(data->rx_hdr, 0x00, sizeof(struct sipc_rx_hdr));
data->skb = NULL;
if (rest)
goto next_frame;
end:
netdev_free_page(data->dev, data->page);
if (rest < 0)
err = -ERANGE;
if (err < 0 && data->skb) {
dev_kfree_skb_any(data->skb);
data->skb = NULL;
}
return err;
}
static void rx_complete(struct urb *req)
{
struct net_device *dev = req->context;
struct usbpn_dev *pnd = netdev_priv(dev);
struct page *page = virt_to_page(req->transfer_buffer);
struct sk_buff *skb;
unsigned long flags;
switch (req->status) {
case 0:
spin_lock_irqsave(&pnd->rx_lock, flags);
skb = pnd->rx_skb;
if (!skb) {
skb = pnd->rx_skb = netdev_alloc_skb(dev, 12);
if (likely(skb)) {
/* Can't use pskb_pull() on page in IRQ */
memcpy(skb_put(skb, 1), page_address(page), 1);
skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
page, 1, req->actual_length);
page = NULL;
}
} else {
skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
page, 0, req->actual_length);
page = NULL;
}
if (req->actual_length < PAGE_SIZE)
pnd->rx_skb = NULL; /* Last fragment */
else
skb = NULL;
spin_unlock_irqrestore(&pnd->rx_lock, flags);
if (skb) {
skb->protocol = htons(ETH_P_PHONET);
skb_reset_mac_header(skb);
__skb_pull(skb, 1);
skb->dev = dev;
dev->stats.rx_packets++;
dev->stats.rx_bytes += skb->len;
netif_rx(skb);
}
goto resubmit;
case -ENOENT:
case -ECONNRESET:
case -ESHUTDOWN:
req = NULL;
break;
case -EOVERFLOW:
dev->stats.rx_over_errors++;
dev_dbg(&dev->dev, "RX overflow\n");
break;
case -EILSEQ:
dev->stats.rx_crc_errors++;
break;
}
dev->stats.rx_errors++;
resubmit:
if (page)
netdev_free_page(dev, page);
if (req)
rx_submit(pnd, req, GFP_ATOMIC);
}
static void rx_complete(struct urb *req)
{
struct usbsvn_rx *svn_rx = req->context;
struct net_device *dev = svn_rx->netdev;
struct usbsvn *svn = netdev_priv(dev);
struct page *page = virt_to_page(req->transfer_buffer);
struct sipc4_rx_data rx_data;
int dev_id = svn_rx->dev_id;
int flags = 0;
int err;
usb_mark_last_busy(svn->usbdev);
switch (req->status) {
case -ENOENT:
if (req->actual_length == 0) {
req = NULL;
break;
}
printk(KERN_DEBUG "%s: Rx ENOENT", __func__);
case 0:
if (!svn->driver_info)
flags |= SIPC4_RX_HDLC;
if (req->actual_length < PAGE_SIZE)
flags |= SIPC4_RX_LAST;
rx_data.dev = dev;
rx_data.skb = svn->devdata[dev_id].rx_skb;
rx_data.page = page;
rx_data.size = req->actual_length;
rx_data.format = dev_id;
rx_data.flags = flags;
rx_data.rx_hdr = &svn->devdata[dev_id].rx_hdr;
page = NULL;
if (rx_debug) {
char *buf = req->transfer_buffer;
int i;
printk(KERN_DEBUG "[RX] dev_id: %d, size: %d\n", dev_id,
req->actual_length);
for (i = 0; i < req->actual_length; i++)
printk(KERN_DEBUG "%x ", *(buf + i));
}
if (dev_id == SIPC4_CMD)
err = usbsvn_cmd_rx(&rx_data, svn);
else
err = sipc4_rx(&rx_data);
if (err < 0) {
svn->devdata[dev_id].rx_skb = NULL;
break;
}
svn->devdata[dev_id].rx_skb = rx_data.skb;
if (dev_id == SIPC4_RAW)
wake_lock_timeout_data(svn);
goto resubmit;
case -ECONNRESET:
case -ESHUTDOWN:
if (!svn->suspended)
printk(KERN_DEBUG "%s: RX complete Status(%d)\n",
__func__, req->status);
req = NULL;
break;
case -EOVERFLOW:
dev->stats.rx_over_errors++;
dev_err(&dev->dev, "RX overflow\n");
break;
case -EILSEQ:
dev->stats.rx_crc_errors++;
break;
}
dev->stats.rx_errors++;
resubmit:
kfree(svn_rx);
if (page)
netdev_free_page(dev, page);
if (req && req->status != -ENOENT) {
rx_submit(svn, dev_id, req, GFP_ATOMIC);
}
}
static int sipc4_hdlc_rx(struct sipc4_rx_data *data)
{
int rest = data->size;
char *buf = page_address(data->page);
int len;
int err;
static __be16 protocol;
int ch;
char *ptr = buf;
int size = rest;
int pos = 0;
int retrycnt = 0;
if (rest <= 0)
goto end;
next_frame:
err = len = sipc4_check_header(data, buf, rest);
if (err < 0) {
_debug_sipc4_print(data, ptr, pos);
goto end;
}
buf += len;
rest -= len;
pos += len;
if (rest == 0)
goto end;
if (rest < 0) {
pr_err("err - r:%d, l:%d, t: %d (%d)\n",
rest, len, retrycnt, __LINE__);
_debug_sipc4_print(data, ptr, pos);
goto end;
}
if (data->format == SIPC4_RAW && len != 0) {
ch = sipc4_get_hdlc_ch(data->skb->data, data->format);
if (ch >= (CHID_PSD_DATA1) && ch <= CHID_PSD_DATA15) {
if ((*buf & 0xF0) == 0x60)
protocol = htons(ETH_P_IPV6);
else if ((*buf & 0xF0) == 0x40)
protocol = htons(ETH_P_IP);
else {
err = -EINVAL;
pr_err("err - ip ver\n");
_debug_sipc4_print(data, ptr, pos);
goto end;
}
}
}
err = len = sipc4_check_data(data, buf, rest, protocol);
if (err < 0) {
pr_err("err - sipc4 data\n");
_debug_sipc4_print(data, ptr, pos);
goto end;
}
buf += len;
rest -= len;
pos += len;
if (rest == 0) /* this packet is splitted */
goto end;
if (rest < 0) {
pr_err("err - r:%d, l:%d, t: %d (%d)\n",
rest, len, retrycnt, __LINE__);
_debug_sipc4_print(data, ptr, pos);
goto end;
}
err = len = sipc4_check_hdlc_end(data, buf, size);
if (err < 0) {
pr_err("err - r:%d, l:%d, t: %d (%d)\n",
rest, len, retrycnt, __LINE__);
_debug_sipc4_print(data, ptr, pos);
goto end;
}
buf += len;
rest -= len;
pos += len;
if (rest < 0) {
pr_err("err - r:%d, l:%d, t: %d (%d)\n",
rest, len, retrycnt, __LINE__);
_debug_sipc4_print(data, ptr, pos);
goto end;
}
err = sipc4_hdlc_format_rx(data, protocol);
if (err < 0) {
pr_err("err - hdlc rx\n");
_debug_sipc4_print(data, ptr, pos);
goto end;
}
data->skb = NULL;
if (rest) {
retrycnt++;
goto next_frame;
}
end:
netdev_free_page(data->dev, data->page);
if (rest < 0)
err = -ERANGE;
if (err < 0 && data->skb) {
dev_kfree_skb_any(data->skb);
data->skb = NULL;
}
return err;
}
