subsys/net/l2/openthread/openthread_utils.c

/*
 * Copyright (c) 2018 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <logging/log.h>
LOG_MODULE_DECLARE(net_l2_openthread, CONFIG_OPENTHREAD_L2_LOG_LEVEL);

#include <net/net_core.h>
#include <net/net_pkt.h>
#include <net/openthread.h>

#include <openthread/ip6.h>
#include <openthread/thread.h>

#include "openthread_utils.h"

#define ALOC16_MASK 0xfc

static bool is_anycast_locator(const otNetifAddress *address)
{
	return address->mAddress.mFields.m16[4] == htons(0x0000) &&
	       address->mAddress.mFields.m16[5] == htons(0x00ff) &&
	       address->mAddress.mFields.m16[6] == htons(0xfe00) &&
	       address->mAddress.mFields.m8[14] == ALOC16_MASK;
}

static bool is_mesh_local(struct openthread_context *context,
			  const u8_t *address)
{
	const otMeshLocalPrefix *ml_prefix =
				otThreadGetMeshLocalPrefix(context->instance);

	return (memcmp(address, ml_prefix->m8, sizeof(ml_prefix)) == 0);
}

int pkt_list_add(struct openthread_context *context, struct net_pkt *pkt)
{
	u16_t i_idx = context->pkt_list_in_idx;

	if (context->pkt_list_full) {
		return -ENOMEM;
	}

	i_idx++;
	if (i_idx == CONFIG_OPENTHREAD_PKT_LIST_SIZE) {
		i_idx = 0U;
	}

	if (i_idx == context->pkt_list_out_idx) {
		context->pkt_list_full = 1U;
	}

	context->pkt_list[context->pkt_list_in_idx].pkt = pkt;
	context->pkt_list_in_idx = i_idx;

	return 0;
}

struct net_pkt *pkt_list_peek(struct openthread_context *context)
{
	if ((context->pkt_list_in_idx == context->pkt_list_out_idx) &&
	    (!context->pkt_list_full)) {

		return NULL;
	}
	return context->pkt_list[context->pkt_list_out_idx].pkt;
}

void pkt_list_remove_last(struct openthread_context *context)
{
	if ((context->pkt_list_in_idx == context->pkt_list_out_idx) &&
	    (!context->pkt_list_full)) {

		return;
	}

	context->pkt_list_out_idx++;
	if (context->pkt_list_out_idx == CONFIG_OPENTHREAD_PKT_LIST_SIZE) {
		context->pkt_list_out_idx = 0U;
	}

	context->pkt_list_full = 0U;
}

void add_ipv6_addr_to_zephyr(struct openthread_context *context)
{
	const otNetifAddress *address;
	struct net_if_addr *if_addr;

	for (address = otIp6GetUnicastAddresses(context->instance);
	     address; address = address->mNext) {

		if (address->mRloc || is_anycast_locator(address)) {
			continue;
		}

		if (CONFIG_OPENTHREAD_L2_LOG_LEVEL == LOG_LEVEL_DBG) {
			char buf[NET_IPV6_ADDR_LEN];

			NET_DBG("Adding %s",
				log_strdup(net_addr_ntop(AF_INET6,
				       (struct in6_addr *)(&address->mAddress),
				       buf, sizeof(buf))));
		}

		if_addr = net_if_ipv6_addr_add(
					context->iface,
					(struct in6_addr *)(&address->mAddress),
					NET_ADDR_AUTOCONF, 0);

		if (if_addr == NULL) {
			NET_ERR("Cannot add OpenThread unicast address");
			continue;
		}

		if_addr->is_mesh_local = is_mesh_local(
					context, address->mAddress.mFields.m8);
	}
}

void add_ipv6_addr_to_ot(struct openthread_context *context)
{
	struct net_if *iface = context->iface;
	struct otNetifAddress addr;
	struct net_if_ipv6 *ipv6;
	int i;

	(void)memset(&addr, 0, sizeof(addr));

	if (net_if_config_ipv6_get(iface, &ipv6) < 0) {
		NET_DBG("Cannot allocate IPv6 address");
		return;
	}

	/* save the last added IP address for this interface */
	for (i = NET_IF_MAX_IPV6_ADDR - 1; i >= 0; i--) {
		if (ipv6->unicast[i].is_used) {
			memcpy(&addr.mAddress,
			       &ipv6->unicast[i].address.in6_addr,
			       sizeof(addr.mAddress));
			break;
		}
	}

	ipv6->unicast[i].is_mesh_local = is_mesh_local(
			context, ipv6->unicast[i].address.in6_addr.s6_addr);

	addr.mValid = true;
	addr.mPreferred = true;
	addr.mPrefixLength = 64;

	otIp6AddUnicastAddress(context->instance, &addr);

	if (CONFIG_OPENTHREAD_L2_LOG_LEVEL == LOG_LEVEL_DBG) {
		char buf[NET_IPV6_ADDR_LEN];

		NET_DBG("Added %s",
			log_strdup(net_addr_ntop(AF_INET6,
						 &addr.mAddress, buf,
						 sizeof(buf))));
	}
}

void add_ipv6_maddr_to_ot(struct openthread_context *context)
{
	struct otIp6Address addr;
	struct net_if_ipv6 *ipv6;
	int i;

	if (net_if_config_ipv6_get(context->iface, &ipv6) < 0) {
		NET_DBG("Cannot allocate IPv6 address");
		return;
	}

	/* save the last added IP address for this interface */
	for (i = NET_IF_MAX_IPV6_MADDR - 1; i >= 0; i--) {
		if (ipv6->mcast[i].is_used) {
			memcpy(&addr,
			       &ipv6->mcast[i].address.in6_addr,
			       sizeof(addr));
			break;
		}
	}

	otIp6SubscribeMulticastAddress(context->instance, &addr);

	if (CONFIG_OPENTHREAD_L2_LOG_LEVEL == LOG_LEVEL_DBG) {
		char buf[NET_IPV6_ADDR_LEN];

		NET_DBG("Added multicast %s",
			log_strdup(net_addr_ntop(AF_INET6, &addr,
						 buf, sizeof(buf))));
	}
}

void add_ipv6_maddr_to_zephyr(struct openthread_context *context)
{
	const otNetifMulticastAddress *maddress;

	for (maddress = otIp6GetMulticastAddresses(context->instance);
	     maddress; maddress = maddress->mNext) {
		if (net_if_ipv6_maddr_lookup(
				(struct in6_addr *)(&maddress->mAddress),
				&context->iface) != NULL) {
			continue;
		}

		if (CONFIG_OPENTHREAD_L2_LOG_LEVEL == LOG_LEVEL_DBG) {
			char buf[NET_IPV6_ADDR_LEN];

			NET_DBG("Adding multicast %s",
				log_strdup(net_addr_ntop(AF_INET6,
							 (struct in6_addr *)
							 (&maddress->mAddress),
							 buf, sizeof(buf))));
		}

		net_if_ipv6_maddr_add(context->iface,
				      (struct in6_addr *)(&maddress->mAddress));
	}
}

void rm_ipv6_addr_from_zephyr(struct openthread_context *context)
{
	struct in6_addr *ot_addr;
	struct net_if_addr *zephyr_addr;
	struct net_if_ipv6 *ipv6;
	int i;

	if (net_if_config_ipv6_get(context->iface, &ipv6) < 0) {
		NET_DBG("Cannot find IPv6 address");
		return;
	}

	for (i = 0; i < NET_IF_MAX_IPV6_ADDR; i++) {
		const otNetifAddress *address;
		bool used = false;

		zephyr_addr = &ipv6->unicast[i];
		if (!zephyr_addr->is_used) {
			continue;
		}

		for (address = otIp6GetUnicastAddresses(context->instance);
		     address; address = address->mNext) {

			ot_addr = (struct in6_addr *)(&address->mAddress);
			if (net_ipv6_addr_cmp(ot_addr,
					      &zephyr_addr->address.in6_addr)) {

				used = true;
				break;
			}
		}
		if (!used) {
			if (CONFIG_OPENTHREAD_L2_LOG_LEVEL == LOG_LEVEL_DBG) {
				char buf[NET_IPV6_ADDR_LEN];

				NET_DBG("Removing %s",
					log_strdup(net_addr_ntop(AF_INET6,
					      &zephyr_addr->address.in6_addr,
					      buf, sizeof(buf))));
			}

			net_if_ipv6_addr_rm(context->iface,
					    &zephyr_addr->address.in6_addr);
		}
	}
}

void rm_ipv6_maddr_from_zephyr(struct openthread_context *context)
{
	struct in6_addr *ot_addr;
	struct net_if_mcast_addr *zephyr_addr;
	struct net_if_ipv6 *ipv6;
	int i;

	if (net_if_config_ipv6_get(context->iface, &ipv6) < 0) {
		NET_DBG("Cannot find IPv6 address");
		return;
	}

	for (i = 0; i < NET_IF_MAX_IPV6_MADDR; i++) {
		const otNetifMulticastAddress *maddress;
		bool used = false;

		zephyr_addr = &ipv6->mcast[i];
		if (!zephyr_addr->is_used) {
			continue;
		}

		for (maddress = otIp6GetMulticastAddresses(context->instance);
		     maddress; maddress = maddress->mNext) {

			ot_addr = (struct in6_addr *)(&maddress->mAddress);
			if (net_ipv6_addr_cmp(ot_addr,
					      &zephyr_addr->address.in6_addr)) {

				used = true;
				break;
			}
		}
		if (!used) {
			if (CONFIG_OPENTHREAD_L2_LOG_LEVEL == LOG_LEVEL_DBG) {
				char buf[NET_IPV6_ADDR_LEN];

				NET_DBG("Removing multicast %s",
					log_strdup(net_addr_ntop(AF_INET6,
					      &zephyr_addr->address.in6_addr,
					      buf, sizeof(buf))));
			}

			net_if_ipv6_maddr_rm(context->iface,
					     &zephyr_addr->address.in6_addr);
		}
	}
}