void net_send_icmp(uint8_t type, uint8_t code, uint8_t *body,
uint16_t body_length) {
/**
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| Type | Code | Checksum |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| |
+ Message Body +
| |
*/
uint8_t buf[2];
CHECK_SP("net_send_icmp: ");
buf[0] = type;
buf[1] = code;
net_send_data(buf, 2);
calc_checksum(buf, 2);
net_send_dummy_checksum();
net_send_data(body, body_length);
calc_checksum(body, body_length);
}
void net_send_icmp_start(uint8_t type, uint8_t code) {
/**
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| Type | Code | Checksum |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| |
+ Message Body +
| |
*/
uint8_t buf[2];
CHECK_SP("net_send_icmp_start: ");
buf[0] = type;
buf[1] = code;
net_send_data(buf, 2);
calc_checksum(buf, 2);
net_send_dummy_checksum();
}
/* Prepare DHCPv4 Message request and send it to peer */
static void send_request(struct net_if *iface, bool renewal)
{
struct net_buf *buf;
buf = prepare_message(iface, DHCPV4_MSG_TYPE_REQUEST);
if (!buf) {
goto fail;
}
if (!add_server_id(buf) ||
!add_req_ipaddr(buf) ||
!add_end(buf)) {
goto fail;
}
setup_header(buf);
if (net_send_data(buf) < 0) {
goto fail;
}
if (renewal) {
iface->dhcpv4.state = NET_DHCPV4_RENEWAL;
} else {
iface->dhcpv4.state = NET_DHCPV4_REQUEST;
}
iface->dhcpv4.attempts++;
k_delayed_work_init(&iface->dhcpv4_timeout, dhcpv4_timeout);
k_delayed_work_submit(&iface->dhcpv4_timeout, get_dhcpv4_timeout());
return;
fail:
NET_DBG("Message preparation failed");
if (!buf) {
net_nbuf_unref(buf);
}
}
/* Prepare DHCPv4 Discover message and broadcast it */
static void send_discover(struct net_if *iface)
{
struct net_buf *buf;
iface->dhcpv4.xid++;
buf = prepare_message(iface, DHCPV4_MSG_TYPE_DISCOVER);
if (!buf) {
goto fail;
}
if (!add_req_options(buf) ||
!add_end(buf)) {
goto fail;
}
setup_header(buf);
if (net_send_data(buf) < 0) {
goto fail;
}
iface->dhcpv4.state = NET_DHCPV4_DISCOVER;
k_delayed_work_init(&iface->dhcpv4_timeout, dhcpv4_timeout);
k_delayed_work_submit(&iface->dhcpv4_timeout, get_dhcpv4_timeout());
return;
fail:
NET_DBG("Message preparation failed");
if (!buf) {
net_nbuf_unref(buf);
}
}
static bool send_iface(struct net_if *iface, int val, bool expect_fail)
{
static u8_t data[] = { 't', 'e', 's', 't', '\0' };
struct net_pkt *pkt;
int ret;
pkt = net_pkt_get_reserve_tx(0, K_FOREVER);
net_pkt_set_iface(pkt, iface);
net_pkt_append_all(pkt, sizeof(data), data, K_FOREVER);
ret = net_send_data(pkt);
if (!expect_fail && ret < 0) {
DBG("Cannot send test packet (%d)\n", ret);
return false;
}
if (!expect_fail && k_sem_take(&wait_data, WAIT_TIME)) {
DBG("Timeout while waiting interface %d data\n", val);
return false;
}
return true;
}
void handle_icmp(uint8_t *macSource, uint8_t *sourceAddr, uint8_t *destIPAddr,
uint16_t length, DATA_CB dataCb, void *priv) {
/**
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| Type | Code | Checksum |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| |
+ Message Body +
| |
*/
/* Allocate just enough data to handle the type, code, and checksum fields */
uint8_t buf[4];
uint8_t type;
CHECK_SP("handle_icmp: ");
#ifdef DEBUG_ICMP
PRINT_SP("handle_icmp: ");
#endif
dataCb(buf, 4, priv);
type = buf[0];
#ifdef DEBUG_ICMP
debug_puts("Payload size: ");
debug_puthex(length);
debug_nl();
debug_puts("ICMP Type:");
debug_puthex(type);
debug_nl();
#endif
/*calc_checksum(payload, 4);
uint16_t r = dataCb(payload, length - 4, priv);
calc_checksum(payload, length - 4);
if (checksum != 0xFFFF) {
debug_puts("Checksum error");
debug_nl();
return;
}*/
if (net_state != STATE_IDLE && type != ICMP_TYPE_NEIGHBOR_SOLICITATION
&& type != ICMP_TYPE_NEIGHBOR_ADVERTISMENT) {
debug_puts("Not in a state to receive ICMP message");
debug_nl();
return;
}
switch (type) {
case ICMP_TYPE_NEIGHBOR_SOLICITATION:
/* First 4 bytes are 'reserved', we ignore them. but must read them */
dataCb(buf, 4, priv);
/* Next 16 bytes are the target address. We assume it's one of our addresses as it was passed to us
by handle_ipv6()
*/
/* We only reply if there is a source link-layer address option */
if (length > 20) {
uint8_t addr[16];
/* Read address*/
dataCb(addr, 16, priv);
/* Read option 'header' */
dataCb(buf, 2, priv);
if (buf[0] == 0x01) {
uint8_t mac_addr[6];
dataCb(mac_addr, 6, priv);
/* We now got the link-layer address and IPv6 address of someone, store it */
register_mac_addr(mac_addr, sourceAddr);
send_neighbor_advertisment(mac_addr, addr, sourceAddr, addr);
}
}
break;
case ICMP_TYPE_NEIGHBOR_ADVERTISMENT: {
/* We ignore first 4 bytes */
uint8_t received_addr[16];
dataCb(buf, 4, priv);
dataCb(received_addr, 16, priv);
if (net_state == STATE_DAD) {
uint8_t addr[16];
net_get_address(ADDRESS_STORE_LINK_LOCAL_OFFSET, addr);
if (memcmp(received_addr, addr, 16) == 0) {
net_state = STATE_INVALID;
return;
}
}
register_mac_addr(macSource, received_addr);
net_state = STATE_IDLE;
}
break;
case ICMP_TYPE_ECHO_REQUEST:
if (length >= 8) {
dataCb(buf, 4, priv);
uint16_t id = (buf[0] << 8) | buf[1];
uint16_t seqNo = (buf[2] << 8) | buf[3];
struct ipv6_packet_arg arg;
arg.dst_mac_addr = null_mac;
arg.dst_ipv6_addr = sourceAddr;
arg.src_ipv6_addr = destIPAddr;
//arg.payload_length = SIZE_ICMP_HEADER + length;
arg.protocol = PROTO_ICMP;
net_start_ipv6_packet(&arg);
net_send_icmp_start(ICMP_TYPE_ECHO_REPLY, 0);
net_send_data(buf, 4);
calc_checksum(buf, 4);
uint16_t count;
length -= 8;
while( (count=dataCb(buf, 4, priv)) > 0 && length > 0) {
net_send_data(buf, count);
calc_checksum(buf, count);
}
//net_send_icmp(ICMP_TYPE_ECHO_REPLY, 0, payload, length);
net_end_ipv6_packet();
}
break;
case ICMP_TYPE_ROUTER_ADVERTISMENT:
/* Ignore first 12 bytes, as we are only interested in
addresses. Next, loop through the options in the payload
*/
{
if( length > 140 ) {
debug_puts("Lengths exceeds 140 Bytes, ignore router advertisment in order to avoid trouble");
debug_nl();
return;
}
uint8_t payload[length-4];
CHECK_SP("handle_icmp, ICMP_TYPE_ROUTER_ADVERTISMENT: ");
dataCb(payload, length-4, priv);
uint8_t *c = payload + 12;
while (c < payload + length - 4) {
if (c[0] == 3) {
uint8_t prefixLength = c[2];
/* Prefix starts at offset 16 */
uint8_t buf[16];
net_get_address(ADDRESS_STORE_MAIN_OFFSET, buf);
if (buf[0] == 0x00) {
// null_mac as destination means link-local (go figure)
routing_table_add(c + 16, prefixLength / 8, null_mac);
// Default route
routing_table_add(unspec_addr, 0, macSource);
assign_address_from_prefix(c + 16, prefixLength);
//mem_write(default_route_mac_id, 0, macSource, 6);
} else {
}
}
c += c[1] * 8;
}
}
break;
}
}
