drivers/net/ethernet/qualcomm/rmnet/rmnet_map_data.c

   1 /* Copyright (c) 2013-2017, The Linux Foundation. All rights reserved.
   2  *
   3  * This program is free software; you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License version 2 and
   5  * only version 2 as published by the Free Software Foundation.
   6  *
   7  * This program is distributed in the hope that it will be useful,
   8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
   9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  10  * GNU General Public License for more details.
  11  *
  12  * RMNET Data MAP protocol
  13  *
  14  */
  15
  16 #include <linux/netdevice.h>
  17 #include "rmnet_config.h"
  18 #include "rmnet_map.h"
  19 #include "rmnet_private.h"
  20
  21 #define RMNET_MAP_DEAGGR_SPACING  64
  22 #define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2)
  23
  24 /* Adds MAP header to front of skb->data
  25  * Padding is calculated and set appropriately in MAP header. Mux ID is
  26  * initialized to 0.
  27  */
  28 struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb,
  29                                                   int hdrlen, int pad)
  30 {
  31         struct rmnet_map_header *map_header;
  32         u32 padding, map_datalen;
  33         u8 *padbytes;
  34
  35         if (skb_headroom(skb) < sizeof(struct rmnet_map_header))
  36                 return NULL;
  37
  38         map_datalen = skb->len - hdrlen;
  39         map_header = (struct rmnet_map_header *)
  40                         skb_push(skb, sizeof(struct rmnet_map_header));
  41         memset(map_header, 0, sizeof(struct rmnet_map_header));
  42
  43         if (pad == RMNET_MAP_NO_PAD_BYTES) {
  44                 map_header->pkt_len = htons(map_datalen);
  45                 return map_header;
  46         }
  47
  48         padding = ALIGN(map_datalen, 4) - map_datalen;
  49
  50         if (padding == 0)
  51                 goto done;
  52
  53         if (skb_tailroom(skb) < padding)
  54                 return NULL;
  55
  56         padbytes = (u8 *)skb_put(skb, padding);
  57         memset(padbytes, 0, padding);
  58
  59 done:
  60         map_header->pkt_len = htons(map_datalen + padding);
  61         map_header->pad_len = padding & 0x3F;
  62
  63         return map_header;
  64 }
  65
  66 /* Deaggregates a single packet
  67  * A whole new buffer is allocated for each portion of an aggregated frame.
  68  * Caller should keep calling deaggregate() on the source skb until 0 is
  69  * returned, indicating that there are no more packets to deaggregate. Caller
  70  * is responsible for freeing the original skb.
  71  */
  72 struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb)
  73 {
  74         struct rmnet_map_header *maph;
  75         struct sk_buff *skbn;
  76         u32 packet_len;
  77
  78         if (skb->len == 0)
  79                 return NULL;
  80
  81         maph = (struct rmnet_map_header *)skb->data;
  82         packet_len = ntohs(maph->pkt_len) + sizeof(struct rmnet_map_header);
  83
  84         if (((int)skb->len - (int)packet_len) < 0)
  85                 return NULL;
  86
  87         /* Some hardware can send us empty frames. Catch them */
  88         if (ntohs(maph->pkt_len) == 0)
  89                 return NULL;
  90
  91         skbn = alloc_skb(packet_len + RMNET_MAP_DEAGGR_SPACING, GFP_ATOMIC);
  92         if (!skbn)
  93                 return NULL;
  94
  95         skbn->dev = skb->dev;
  96         skb_reserve(skbn, RMNET_MAP_DEAGGR_HEADROOM);
  97         skb_put(skbn, packet_len);
  98         memcpy(skbn->data, skb->data, packet_len);
  99         skb_pull(skb, packet_len);
 100
 101         return skbn;
 102 }