diff options
| -rw-r--r-- | drivers/net/gtp.c | 41 | ||||
| -rw-r--r-- | include/net/gtp.h | 5 |
2 files changed, 46 insertions, 0 deletions
diff --git a/drivers/net/gtp.c b/drivers/net/gtp.c index dffb99a97e0f..0522b20242ba 100644 --- a/drivers/net/gtp.c +++ b/drivers/net/gtp.c @@ -567,6 +567,43 @@ static int gtp1u_handle_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb) msg, 0, GTP_GENL_MCGRP, GFP_ATOMIC); } +static int gtp_parse_exthdrs(struct sk_buff *skb, unsigned int *hdrlen) +{ + struct gtp_ext_hdr *gtp_exthdr, _gtp_exthdr; + unsigned int offset = *hdrlen; + __u8 *next_type, _next_type; + + /* From 29.060: "The Extension Header Length field specifies the length + * of the particular Extension header in 4 octets units." + * + * This length field includes length field size itself (1 byte), + * payload (variable length) and next type (1 byte). The extension + * header is aligned to to 4 bytes. + */ + + do { + gtp_exthdr = skb_header_pointer(skb, offset, sizeof(*gtp_exthdr), + &_gtp_exthdr); + if (!gtp_exthdr || !gtp_exthdr->len) + return -1; + + offset += gtp_exthdr->len * 4; + + /* From 29.060: "If no such Header follows, then the value of + * the Next Extension Header Type shall be 0." + */ + next_type = skb_header_pointer(skb, offset - 1, + sizeof(_next_type), &_next_type); + if (!next_type) + return -1; + + } while (*next_type != 0); + + *hdrlen = offset; + + return 0; +} + static int gtp1u_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb) { unsigned int hdrlen = sizeof(struct udphdr) + @@ -616,6 +653,10 @@ static int gtp1u_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb) return 1; } + if (gtp1->flags & GTP1_F_EXTHDR && + gtp_parse_exthdrs(skb, &hdrlen) < 0) + return -1; + return gtp_rx(pctx, skb, hdrlen, gtp->role); } diff --git a/include/net/gtp.h b/include/net/gtp.h index 2a503f035d18..c0253c8702d0 100644 --- a/include/net/gtp.h +++ b/include/net/gtp.h @@ -78,4 +78,9 @@ static inline bool netif_is_gtp(const struct net_device *dev) #define GTP1_F_EXTHDR 0x04 #define GTP1_F_MASK 0x07 +struct gtp_ext_hdr { + __u8 len; + __u8 data[]; +}; + #endif |