static int sipc4_fmt_rx(struct sipc4_rx_data *data)
{
struct net_device *dev = data->dev;
struct sk_buff *skb = data->skb;
int ch = sipc4_get_hdlc_ch(skb->data, SIPC4_FMT);
int control = sipc4_get_hdlc_control(skb->data, SIPC4_FMT);
int info_id = control & FMT_INFOID_MASK;
/* Remove header */
skb_pull(skb, sipc4_get_header_size(SIPC4_FMT));
if (control & FMT_MORE_BIT) {
skb_queue_tail(&fmt_multi_list[info_id], skb);
return 0;
}
if (fmt_multi_list[info_id].qlen) {
/* TODO: multi frame */
;
}
sipc4_netif_rx(dev, skb, SIPC4_RES(SIPC4_FMT, ch));
return 0;
}
static int sipc4_rfs_rx(struct sipc4_rx_data *data)
{
struct net_device *dev = data->dev;
struct sk_buff *skb = data->skb;
struct sipc_rx_hdr *hdr = data->rx_hdr;
int ch = sipc4_get_hdlc_ch(hdr->hdr, SIPC4_RFS);
return sipc4_netif_rx(dev, skb, SIPC4_RES(SIPC4_RFS, ch));
}
static int sipc4_rfs_rx(struct sipc4_rx_data *data)
{
struct net_device *dev = data->dev;
struct sk_buff *skb = data->skb;
struct sipc4_rx_frag *frag = (struct sipc4_rx_frag *)skb->cb;
int ch = sipc4_get_hdlc_ch(skb->data, SIPC4_RFS);
/* Remove header */
if (frag->data_len)
skb_pull(skb, sipc4_get_header_size(SIPC4_RFS));
return sipc4_netif_rx(dev, skb, SIPC4_RES(SIPC4_RFS, ch));
}
static int sipc4_raw_rx(struct sipc4_rx_data *data)
{
struct net_device *dev = data->dev;
struct sk_buff *skb = data->skb;
struct sipc_rx_hdr *hdr = data->rx_hdr;
int ch = sipc4_get_hdlc_ch(hdr->hdr, SIPC4_RAW);
/* check pdp channel */
if (ch >= CHID_PSD_DATA1 && ch <= CHID_PSD_DATA15)
pdp_netif_rx(dev, skb, ch);
else
sipc4_netif_rx(dev, skb, SIPC4_RES(SIPC4_RAW, ch));
return 0;
}
static int sipc4_raw_rx(struct sipc4_rx_data *data,
const __be16 protocol)
{
struct net_device *dev = data->dev;
struct sk_buff *skb = data->skb;
int ch = sipc4_get_hdlc_ch(skb->data, SIPC4_RAW);
/* Remove header */
skb_pull(skb, sipc4_get_header_size(SIPC4_RAW));
/* check pdp channel */
if (ch >= (CHID_PSD_DATA1) && ch <= CHID_PSD_DATA15)
pdp_netif_rx(dev, skb, ch, protocol);
else
sipc4_netif_rx(dev, skb, SIPC4_RES(SIPC4_RAW, ch));
return 0;
}
static int sipc4_fmt_rx(struct sipc4_rx_data *data)
{
struct net_device *dev = data->dev;
struct sk_buff *skb = data->skb;
struct sipc_rx_hdr *hdr = data->rx_hdr;
int ch = sipc4_get_hdlc_ch(hdr->hdr, SIPC4_FMT);
int control = sipc4_get_hdlc_control(hdr->hdr, SIPC4_FMT);
int info_id = control & FMT_INFOID_MASK;
if (control & FMT_MORE_BIT) {
skb_queue_tail(&fmt_multi_list[info_id], skb);
return 0;
}
if (fmt_multi_list[info_id].qlen) {
/* TODO: multi frame */
;
}
sipc4_netif_rx(dev, skb, SIPC4_RES(SIPC4_FMT, ch));
return 0;
}
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;
}
