103 lines
2.8 KiB
C
103 lines
2.8 KiB
C
|
/* Copyright (c) 2013-2017, The Linux Foundation. All rights reserved.
|
||
|
|
*
|
||
|
|
* This program is free software; you can redistribute it and/or modify
|
||
|
|
* it under the terms of the GNU General Public License version 2 and
|
||
|
|
* only version 2 as published by the Free Software Foundation.
|
||
|
|
*
|
||
|
|
* 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.
|
||
|
|
*
|
||
|
|
* RMNET Data MAP protocol
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <linux/netdevice.h>
|
||
|
|
#include "rmnet_config.h"
|
||
|
|
#include "rmnet_map.h"
|
||
|
|
#include "rmnet_private.h"
|
||
|
|
|
||
|
|
#define RMNET_MAP_DEAGGR_SPACING 64
|
||
|
|
#define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2)
|
||
|
|
|
||
|
|
/* Adds MAP header to front of skb->data
|
||
|
|
* Padding is calculated and set appropriately in MAP header. Mux ID is
|
||
|
|
* initialized to 0.
|
||
|
|
*/
|
||
|
|
struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb,
|
||
|
|
int hdrlen, int pad)
|
||
|
|
{
|
||
|
|
struct rmnet_map_header *map_header;
|
||
|
|
u32 padding, map_datalen;
|
||
|
|
u8 *padbytes;
|
||
|
|
|
||
|
|
if (skb_headroom(skb) < sizeof(struct rmnet_map_header))
|
||
|
|
return NULL;
|
||
|
|
|
||
|
|
map_datalen = skb->len - hdrlen;
|
||
|
|
map_header = (struct rmnet_map_header *)
|
||
|
|
skb_push(skb, sizeof(struct rmnet_map_header));
|
||
|
|
memset(map_header, 0, sizeof(struct rmnet_map_header));
|
||
|
|
|
||
|
|
if (pad == RMNET_MAP_NO_PAD_BYTES) {
|
||
|
|
map_header->pkt_len = htons(map_datalen);
|
||
|
|
return map_header;
|
||
|
|
}
|
||
|
|
|
||
|
|
padding = ALIGN(map_datalen, 4) - map_datalen;
|
||
|
|
|
||
|
|
if (padding == 0)
|
||
|
|
goto done;
|
||
|
|
|
||
|
|
if (skb_tailroom(skb) < padding)
|
||
|
|
return NULL;
|
||
|
|
|
||
|
|
padbytes = (u8 *)skb_put(skb, padding);
|
||
|
|
memset(padbytes, 0, padding);
|
||
|
|
|
||
|
|
done:
|
||
|
|
map_header->pkt_len = htons(map_datalen + padding);
|
||
|
|
map_header->pad_len = padding & 0x3F;
|
||
|
|
|
||
|
|
return map_header;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Deaggregates a single packet
|
||
|
|
* A whole new buffer is allocated for each portion of an aggregated frame.
|
||
|
|
* Caller should keep calling deaggregate() on the source skb until 0 is
|
||
|
|
* returned, indicating that there are no more packets to deaggregate. Caller
|
||
|
|
* is responsible for freeing the original skb.
|
||
|
|
*/
|
||
|
|
struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb)
|
||
|
|
{
|
||
|
|
struct rmnet_map_header *maph;
|
||
|
|
struct sk_buff *skbn;
|
||
|
|
u32 packet_len;
|
||
|
|
|
||
|
|
if (skb->len == 0)
|
||
|
|
return NULL;
|
||
|
|
|
||
|
|
maph = (struct rmnet_map_header *)skb->data;
|
||
|
|
packet_len = ntohs(maph->pkt_len) + sizeof(struct rmnet_map_header);
|
||
|
|
|
||
|
|
if (((int)skb->len - (int)packet_len) < 0)
|
||
|
|
return NULL;
|
||
|
|
|
||
|
|
/* Some hardware can send us empty frames. Catch them */
|
||
|
|
if (ntohs(maph->pkt_len) == 0)
|
||
|
|
return NULL;
|
||
|
|
|
||
|
|
skbn = alloc_skb(packet_len + RMNET_MAP_DEAGGR_SPACING, GFP_ATOMIC);
|
||
|
|
if (!skbn)
|
||
|
|
return NULL;
|
||
|
|
|
||
|
|
skbn->dev = skb->dev;
|
||
|
|
skb_reserve(skbn, RMNET_MAP_DEAGGR_HEADROOM);
|
||
|
|
skb_put(skbn, packet_len);
|
||
|
|
memcpy(skbn->data, skb->data, packet_len);
|
||
|
|
skb_pull(skb, packet_len);
|
||
|
|
|
||
|
|
return skbn;
|
||
|
|
}
|