static int sipc4_check_data(struct sipc4_rx_data *data, char *buf, int rest)
{
struct net_device *dev = data->dev;
struct sk_buff *skb = data->skb;
struct sipc_rx_hdr *hdr = data->rx_hdr;
int head_size = sipc4_get_header_size(data->format);
int data_size = sipc4_get_hdlc_size(hdr->hdr, data->format) - head_size;
int alloc_size = data_size;
int len;
struct sk_buff *skb_new;
int err;
if (!skb) {
switch (data->format) {
case SIPC4_RFS:
/* Send the RFS header to RILD*/
alloc_size = sipc_min(data_size, rest) + head_size;
skb = sipc4_alloc_skb(dev, alloc_size);
if (unlikely(!skb))
return -ENOMEM;
memcpy(skb_put(skb, head_size), hdr->hdr, head_size);
break;
default:
skb = sipc4_alloc_skb(dev, alloc_size);
if (unlikely(!skb))
return -ENOMEM;
break;
}
data->skb = skb;
} else {
switch (data->format) {
case SIPC4_RFS:
alloc_size = sipc_min(data_size - hdr->flag_len, rest);
skb_new = sipc4_alloc_skb(data->dev, alloc_size);
if (unlikely(!skb_new))
return -ENOMEM;
err = sipc4_hdlc_format_rx(data);
if (err < 0) {
dev_kfree_skb_any(skb_new);
return err;
}
skb = data->skb = skb_new;
break;
default:
/* do noting, IPC,RAW MTU is 1500byte */
break;
}
}
/* handle data */
len = sipc_min(rest, alloc_size - skb->len);
memcpy(skb_put(skb, len), buf, len);
hdr->flag_len += len;
return len;
}
static int sipc4_check_data(struct sipc4_rx_data *data, char *buf, int rest,
const __be16 protocol)
{
struct sk_buff *skb = data->skb;
struct sipc4_rx_frag *frag = (struct sipc4_rx_frag *)skb->cb;
struct page *page;
int header_size = sipc4_get_header_size(data->format);
int hdlc_size = sipc4_get_hdlc_size(skb->data, data->format);
int data_size = hdlc_size - header_size;
int skb_data_size = skb->data_len + frag->data_len;
int rest_data_size = data_size - skb_data_size;
int len;
int skb_max_pages = MAX_SKB_FRAGS;
len = rest < rest_data_size ? rest : rest_data_size;
page = __netdev_alloc_page(data->dev, GFP_ATOMIC);
if (!page) {
pr_err("%s - failed to alloc netdev page\n", __func__);
return -ENOMEM;
}
if (data->format == SIPC4_RFS)
skb_max_pages = 1;
/* check fragment number */
if (skb_shinfo(skb)->nr_frags >= skb_max_pages) {
struct sk_buff *skb_new;
int err;
skb_new = sipc4_alloc_skb(data, header_size);
if (!skb) {
pr_err("%s - failed to alloc skb\n", __func__);
return -ENOMEM;
}
memcpy(skb_put(skb_new, header_size), skb->data, header_size);
memcpy(skb_new->cb, skb->cb, sizeof(struct sipc4_rx_frag));
frag = (struct sipc4_rx_frag *)skb_new->cb;
frag->data_len += skb->data_len;
err = sipc4_hdlc_format_rx(data, protocol);
if (err < 0) {
pr_err("%s - failed to rx data\n", __func__);
dev_kfree_skb_any(skb_new);
return err;
}
skb = data->skb = skb_new;
}
/* handle data */
memcpy(page_address(page), buf, len);
skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page, 0, len);
return len;
}
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 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;
}