gnrc_rpl_srh.c
3.14 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
/*
* Copyright (C) 2015 Martine Lenders <mlenders@inf.fu-berlin.de>
*
* This file is subject to the terms and conditions of the GNU Lesser
* General Public License v2.1. See the file LICENSE in the top level
* directory for more details.
*/
/**
* @{
*
* @file
*/
#include <string.h>
#include "net/gnrc/ipv6/netif.h"
#include "net/gnrc/rpl/srh.h"
#define ENABLE_DEBUG (0)
#include "debug.h"
#if ENABLE_DEBUG
static char addr_str[IPV6_ADDR_MAX_STR_LEN];
#endif
#define GNRC_RPL_SRH_PADDING(X) ((X & 0xF0) >> 4)
#define GNRC_RPL_SRH_COMPRE(X) (X & 0x0F)
#define GNRC_RPL_SRH_COMPRI(X) ((X & 0xF0) >> 4)
int gnrc_rpl_srh_process(ipv6_hdr_t *ipv6, gnrc_rpl_srh_t *rh)
{
if (rh->seg_left == 0) {
return EXT_RH_CODE_OK;
}
uint8_t n = (((rh->len * 8) - GNRC_RPL_SRH_PADDING(rh->pad_resv) -
(16 - GNRC_RPL_SRH_COMPRE(rh->compr))) /
(16 - GNRC_RPL_SRH_COMPRI(rh->compr))) + 1;
ipv6_addr_t addr = ipv6->dst, tmp;
uint8_t i, pref_elided, tmp_pref_elided, addr_len, compri_addr_len, tmp_addr_len, found_pos = 0;
uint8_t *addr_vec = (uint8_t *) (rh + 1);
bool found = false;
DEBUG("RPL SRH: %u addresses in the routing header\n", (unsigned) n);
if (rh->seg_left > n) {
DEBUG("RPL SRH: number of segments left > number of addresses - discard\n");
/* TODO ICMP Parameter Problem - Code 0 */
return EXT_RH_CODE_ERROR;
}
rh->seg_left--;
i = n - rh->seg_left;
pref_elided = rh->seg_left ? GNRC_RPL_SRH_COMPRI(rh->compr) : GNRC_RPL_SRH_COMPRE(rh->compr);
compri_addr_len = sizeof(ipv6_addr_t) - GNRC_RPL_SRH_COMPRI(rh->compr);
addr_len = sizeof(ipv6_addr_t) - pref_elided;
memcpy(&addr.u8[pref_elided], &addr_vec[(i - 1) * compri_addr_len], addr_len);
if (ipv6_addr_is_multicast(&ipv6->dst) || ipv6_addr_is_multicast(&addr)) {
DEBUG("RPL SRH: found a multicast address - discard\n");
/* TODO discard the packet */
return EXT_RH_CODE_ERROR;
}
/* check if multiple addresses of my interface exist */
tmp_pref_elided = GNRC_RPL_SRH_COMPRI(rh->compr);
tmp_addr_len = sizeof(ipv6_addr_t) - tmp_pref_elided;
tmp = ipv6->dst;
for (uint8_t k = 0; k < n; k++) {
if (k == n - 1) {
tmp_pref_elided = GNRC_RPL_SRH_COMPRE(rh->compr);
tmp_addr_len = sizeof(ipv6_addr_t) - tmp_pref_elided;
}
memcpy(&tmp.u8[tmp_pref_elided], &addr_vec[k * compri_addr_len], tmp_addr_len);
if (gnrc_ipv6_netif_find_by_addr(NULL, &tmp) != KERNEL_PID_UNDEF) {
if (found && ((k - found_pos) > 1)) {
DEBUG("RPL SRH: found multiple addresses that belong to me - discard\n");
/* TODO send an ICMP Parameter Problem (Code 0) and discard the packet */
return EXT_RH_CODE_ERROR;
}
found_pos = k;
found = true;
}
}
memcpy(&addr_vec[(i - 1) * compri_addr_len], &ipv6->dst.u8[pref_elided], addr_len);
DEBUG("RPL SRH: Next hop: %s at position %d\n",
ipv6_addr_to_str(addr_str, &addr, sizeof(addr_str)), i);
ipv6->dst = addr;
return EXT_RH_CODE_FORWARD;
}
/** @} */