1 /* 2 * 6LoWPAN next header compression 3 * 4 * 5 * Authors: 6 * Alexander Aring <aar@pengutronix.de> 7 * 8 * This program is free software; you can redistribute it and/or 9 * modify it under the terms of the GNU General Public License 10 * as published by the Free Software Foundation; either version 11 * 2 of the License, or (at your option) any later version. 12 */ 13 14 #include <linux/netdevice.h> 15 16 #include <net/ipv6.h> 17 18 #include "nhc.h" 19 20 static struct rb_root rb_root = RB_ROOT; 21 static struct lowpan_nhc *lowpan_nexthdr_nhcs[NEXTHDR_MAX]; 22 static DEFINE_SPINLOCK(lowpan_nhc_lock); 23 24 static int lowpan_nhc_insert(struct lowpan_nhc *nhc) 25 { 26 struct rb_node **new = &rb_root.rb_node, *parent = NULL; 27 28 /* Figure out where to put new node */ 29 while (*new) { 30 struct lowpan_nhc *this = container_of(*new, struct lowpan_nhc, 31 node); 32 int result, len_dif, len; 33 34 len_dif = nhc->idlen - this->idlen; 35 36 if (nhc->idlen < this->idlen) 37 len = nhc->idlen; 38 else 39 len = this->idlen; 40 41 result = memcmp(nhc->id, this->id, len); 42 if (!result) 43 result = len_dif; 44 45 parent = *new; 46 if (result < 0) 47 new = &((*new)->rb_left); 48 else if (result > 0) 49 new = &((*new)->rb_right); 50 else 51 return -EEXIST; 52 } 53 54 /* Add new node and rebalance tree. */ 55 rb_link_node(&nhc->node, parent, new); 56 rb_insert_color(&nhc->node, &rb_root); 57 58 return 0; 59 } 60 61 static void lowpan_nhc_remove(struct lowpan_nhc *nhc) 62 { 63 rb_erase(&nhc->node, &rb_root); 64 } 65 66 static struct lowpan_nhc *lowpan_nhc_by_nhcid(const struct sk_buff *skb) 67 { 68 struct rb_node *node = rb_root.rb_node; 69 const u8 *nhcid_skb_ptr = skb->data; 70 71 while (node) { 72 struct lowpan_nhc *nhc = container_of(node, struct lowpan_nhc, 73 node); 74 u8 nhcid_skb_ptr_masked[LOWPAN_NHC_MAX_ID_LEN]; 75 int result, i; 76 77 if (nhcid_skb_ptr + nhc->idlen > skb->data + skb->len) 78 return NULL; 79 80 /* copy and mask afterwards the nhid value from skb */ 81 memcpy(nhcid_skb_ptr_masked, nhcid_skb_ptr, nhc->idlen); 82 for (i = 0; i < nhc->idlen; i++) 83 nhcid_skb_ptr_masked[i] &= nhc->idmask[i]; 84 85 result = memcmp(nhcid_skb_ptr_masked, nhc->id, nhc->idlen); 86 if (result < 0) 87 node = node->rb_left; 88 else if (result > 0) 89 node = node->rb_right; 90 else 91 return nhc; 92 } 93 94 return NULL; 95 } 96 97 int lowpan_nhc_check_compression(struct sk_buff *skb, 98 const struct ipv6hdr *hdr, u8 **hc_ptr, 99 u8 *iphc0) 100 { 101 struct lowpan_nhc *nhc; 102 103 spin_lock_bh(&lowpan_nhc_lock); 104 105 nhc = lowpan_nexthdr_nhcs[hdr->nexthdr]; 106 if (nhc && nhc->compress) 107 *iphc0 |= LOWPAN_IPHC_NH_C; 108 else 109 lowpan_push_hc_data(hc_ptr, &hdr->nexthdr, 110 sizeof(hdr->nexthdr)); 111 112 spin_unlock_bh(&lowpan_nhc_lock); 113 114 return 0; 115 } 116 117 int lowpan_nhc_do_compression(struct sk_buff *skb, const struct ipv6hdr *hdr, 118 u8 **hc_ptr) 119 { 120 int ret; 121 struct lowpan_nhc *nhc; 122 123 spin_lock_bh(&lowpan_nhc_lock); 124 125 nhc = lowpan_nexthdr_nhcs[hdr->nexthdr]; 126 /* check if the nhc module was removed in unlocked part. 127 * TODO: this is a workaround we should prevent unloading 128 * of nhc modules while unlocked part, this will always drop 129 * the lowpan packet but it's very unlikely. 130 * 131 * Solution isn't easy because we need to decide at 132 * lowpan_nhc_check_compression if we do a compression or not. 133 * Because the inline data which is added to skb, we can't move this 134 * handling. 135 */ 136 if (unlikely(!nhc || !nhc->compress)) { 137 ret = -EINVAL; 138 goto out; 139 } 140 141 /* In the case of RAW sockets the transport header is not set by 142 * the ip6 stack so we must set it ourselves 143 */ 144 if (skb->transport_header == skb->network_header) 145 skb_set_transport_header(skb, sizeof(struct ipv6hdr)); 146 147 ret = nhc->compress(skb, hc_ptr); 148 if (ret < 0) 149 goto out; 150 151 /* skip the transport header */ 152 skb_pull(skb, nhc->nexthdrlen); 153 154 out: 155 spin_unlock_bh(&lowpan_nhc_lock); 156 157 return ret; 158 } 159 160 int lowpan_nhc_do_uncompression(struct sk_buff *skb, struct net_device *dev, 161 struct ipv6hdr *hdr) 162 { 163 struct lowpan_nhc *nhc; 164 int ret; 165 166 spin_lock_bh(&lowpan_nhc_lock); 167 168 nhc = lowpan_nhc_by_nhcid(skb); 169 if (nhc) { 170 if (nhc->uncompress) { 171 ret = nhc->uncompress(skb, sizeof(struct ipv6hdr) + 172 nhc->nexthdrlen); 173 if (ret < 0) { 174 spin_unlock_bh(&lowpan_nhc_lock); 175 return ret; 176 } 177 } else { 178 spin_unlock_bh(&lowpan_nhc_lock); 179 netdev_warn(dev, "received nhc id for %s which is not implemented.\n", 180 nhc->name); 181 return -ENOTSUPP; 182 } 183 } else { 184 spin_unlock_bh(&lowpan_nhc_lock); 185 netdev_warn(dev, "received unknown nhc id which was not found.\n"); 186 return -ENOENT; 187 } 188 189 hdr->nexthdr = nhc->nexthdr; 190 skb_reset_transport_header(skb); 191 raw_dump_table(__func__, "raw transport header dump", 192 skb_transport_header(skb), nhc->nexthdrlen); 193 194 spin_unlock_bh(&lowpan_nhc_lock); 195 196 return 0; 197 } 198 199 int lowpan_nhc_add(struct lowpan_nhc *nhc) 200 { 201 int ret; 202 203 if (!nhc->idlen || !nhc->idsetup) 204 return -EINVAL; 205 206 WARN_ONCE(nhc->idlen > LOWPAN_NHC_MAX_ID_LEN, 207 "LOWPAN_NHC_MAX_ID_LEN should be updated to %zd.\n", 208 nhc->idlen); 209 210 nhc->idsetup(nhc); 211 212 spin_lock_bh(&lowpan_nhc_lock); 213 214 if (lowpan_nexthdr_nhcs[nhc->nexthdr]) { 215 ret = -EEXIST; 216 goto out; 217 } 218 219 ret = lowpan_nhc_insert(nhc); 220 if (ret < 0) 221 goto out; 222 223 lowpan_nexthdr_nhcs[nhc->nexthdr] = nhc; 224 out: 225 spin_unlock_bh(&lowpan_nhc_lock); 226 return ret; 227 } 228 EXPORT_SYMBOL(lowpan_nhc_add); 229 230 void lowpan_nhc_del(struct lowpan_nhc *nhc) 231 { 232 spin_lock_bh(&lowpan_nhc_lock); 233 234 lowpan_nhc_remove(nhc); 235 lowpan_nexthdr_nhcs[nhc->nexthdr] = NULL; 236 237 spin_unlock_bh(&lowpan_nhc_lock); 238 239 synchronize_net(); 240 } 241 EXPORT_SYMBOL(lowpan_nhc_del); 242