mesh: Baseline Mesh implementation

1 files changed, 369 insertions, 0 deletions

diff --git a/mesh/util.c b/mesh/util.c
new file mode 100644
index 000000000..cb241b3cc
--- /dev/null
+++ b/mesh/util.c
@@ -0,0 +1,369 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2017  Intel Corporation. All rights reserved.
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdbool.h>
+#include <inttypes.h>
+#include <readline/readline.h>
+#include <glib.h>
+
+#include "client/display.h"
+#include "src/shared/util.h"
+#include "mesh-net.h"
+#include "util.h"
+
+struct cmd_menu {
+	const char *name;
+	const struct menu_entry *table;
+};
+
+static struct menu_entry *main_cmd_table;
+static struct menu_entry *current_cmd_table;
+static GList *menu_list;
+
+static char *main_menu_prompt;
+static int main_menu_point;
+
+static int match_menu_name(const void *a, const void *b)
+{
+	const struct cmd_menu *menu = a;
+	const char *name = b;
+
+	return strcasecmp(menu->name, name);
+}
+
+bool cmd_menu_init(const struct menu_entry *cmd_table)
+{
+	struct cmd_menu *menu;
+
+	if (main_cmd_table) {
+		rl_printf("Main menu already registered\n");
+		return false;
+	}
+
+	menu = g_malloc(sizeof(struct cmd_menu));
+	if (!menu)
+		return false;
+
+	menu->name = "meshctl";
+	menu->table = cmd_table;
+	menu_list = g_list_append(menu_list, menu);
+	main_cmd_table = (struct menu_entry *) cmd_table;
+	current_cmd_table = (struct menu_entry *) main_cmd_table;
+
+	return true;
+}
+
+void cmd_menu_main(bool forced)
+{
+	current_cmd_table = main_cmd_table;
+
+	if (!forced) {
+		rl_set_prompt(main_menu_prompt);
+		rl_replace_line("", 0);
+		rl_point = main_menu_point;
+		rl_redisplay();
+	}
+
+	g_free(main_menu_prompt);
+	main_menu_prompt = NULL;
+}
+
+bool add_cmd_menu(const char *name, const struct menu_entry *cmd_table)
+{
+	struct cmd_menu *menu;
+	GList *l;
+
+	l = g_list_find_custom(menu_list, name, match_menu_name);
+	if (l) {
+		menu = l->data;
+		rl_printf("menu \"%s\" already registered\n", menu->name);
+		return false;
+	}
+
+	menu = g_malloc(sizeof(struct cmd_menu));
+	if (!menu)
+		return false;
+
+	menu->name = name;
+	menu->table = cmd_table;
+	menu_list = g_list_append(menu_list, menu);
+
+	return true;
+}
+
+void set_menu_prompt(const char *name, const char *id)
+{
+	char *prompt;
+
+	prompt = g_strdup_printf(COLOR_BLUE "[%s%s%s]" COLOR_OFF "# ", name,
+					id ? ": Target = " : "", id ? id : "");
+	rl_set_prompt(prompt);
+	g_free(prompt);
+	rl_on_new_line();
+}
+
+bool switch_cmd_menu(const char *name)
+{
+	GList *l;
+	struct cmd_menu *menu;
+
+	l = g_list_find_custom(menu_list, name, match_menu_name);
+	if(!l)
+		return false;
+
+	menu = l->data;
+	current_cmd_table = (struct menu_entry *) menu->table;
+
+	main_menu_point = rl_point;
+	main_menu_prompt = g_strdup(rl_prompt);
+
+	return true;
+}
+
+void process_menu_cmd(const char *cmd, const char *arg)
+{
+	int i;
+	int len;
+	struct menu_entry *cmd_table = current_cmd_table;
+
+	if (!current_cmd_table)
+		return;
+
+	len = strlen(cmd);
+
+	for (i = 0; cmd_table[i].cmd; i++) {
+		if (strncmp(cmd, cmd_table[i].cmd, len))
+			continue;
+
+		if (cmd_table[i].func) {
+			cmd_table[i].func(arg);
+			return;
+		}
+	}
+
+	if (strncmp(cmd, "help", len)) {
+		rl_printf("Invalid command\n");
+		return;
+	}
+
+	print_cmd_menu(cmd_table);
+}
+
+void print_cmd_menu(const struct menu_entry *cmd_table)
+{
+	int i;
+
+	rl_printf("Available commands:\n");
+
+	for (i = 0; cmd_table[i].cmd; i++) {
+		if (cmd_table[i].desc)
+			rl_printf("  %s %-*s %s\n", cmd_table[i].cmd,
+					(int)(40 - strlen(cmd_table[i].cmd)),
+					cmd_table[i].arg ? : "",
+					cmd_table[i].desc ? : "");
+	}
+
+}
+
+void cmd_menu_cleanup(void)
+{
+	main_cmd_table = NULL;
+	current_cmd_table = NULL;
+
+	g_list_free_full(menu_list, g_free);
+}
+
+void print_byte_array(const char *prefix, const void *ptr, int len)
+{
+	const uint8_t *data = ptr;
+	char *line, *bytes;
+	int i;
+
+	line = g_malloc(strlen(prefix) + (16 * 3) + 2);
+	sprintf(line, "%s ", prefix);
+	bytes = line + strlen(prefix) + 1;
+
+	for (i = 0; i < len; ++i) {
+		sprintf(bytes, "%2.2x ", data[i]);
+		if ((i + 1) % 16) {
+			bytes += 3;
+		} else {
+			rl_printf("\r%s\n", line);
+			bytes = line + strlen(prefix) + 1;
+		}
+	}
+
+	if (i % 16)
+		rl_printf("\r%s\n", line);
+
+	g_free(line);
+}
+
+bool str2hex(const char *str, uint16_t in_len, uint8_t *out,
+		uint16_t out_len)
+{
+	uint16_t i;
+
+	if (in_len < out_len * 2)
+		return false;
+
+	for (i = 0; i < out_len; i++) {
+		if (sscanf(&str[i * 2], "%02hhx", &out[i]) != 1)
+			return false;
+	}
+
+	return true;
+}
+
+size_t hex2str(uint8_t *in, size_t in_len, char *out,
+		size_t out_len)
+{
+	static const char hexdigits[] = "0123456789abcdef";
+	size_t i;
+
+	if(in_len * 2 > out_len - 1)
+		return 0;
+
+	for (i = 0; i < in_len; i++) {
+		out[i * 2] = hexdigits[in[i] >> 4];
+		out[i * 2 + 1] = hexdigits[in[i] & 0xf];
+	}
+
+	out[in_len * 2] = '\0';
+	return i;
+}
+
+uint16_t mesh_opcode_set(uint32_t opcode, uint8_t *buf)
+{
+	if (opcode <= 0x7e) {
+		buf[0] = opcode;
+		return 1;
+	} else if (opcode >= 0x8000 && opcode <= 0xbfff) {
+		put_be16(opcode, buf);
+		return 2;
+	} else if (opcode >= 0xc00000 && opcode <= 0xffffff) {
+		buf[0] = (opcode >> 16) & 0xff;
+		put_be16(opcode, buf + 1);
+		return 3;
+	} else {
+		rl_printf("Illegal Opcode %x", opcode);
+		return 0;
+	}
+}
+
+bool mesh_opcode_get(const uint8_t *buf, uint16_t sz, uint32_t *opcode, int *n)
+{
+	if (!n || !opcode || sz < 1) return false;
+
+	switch (buf[0] & 0xc0) {
+	case 0x00:
+	case 0x40:
+		/* RFU */
+		if (buf[0] == 0x7f)
+			return false;
+
+		*n = 1;
+		*opcode = buf[0];
+		break;
+
+	case 0x80:
+		if (sz < 2)
+			return false;
+
+		*n = 2;
+		*opcode = get_be16(buf);
+		break;
+
+	case 0xc0:
+		if (sz < 3)
+			return false;
+
+		*n = 3;
+		*opcode = get_be16(buf + 1);
+		*opcode |= buf[0] << 16;
+		break;
+
+	default:
+		rl_printf("Bad Packet:\n");
+		print_byte_array("\t", (void *) buf, sz);
+		return false;
+	}
+
+	return true;
+}
+
+const char *mesh_status_str(uint8_t status)
+{
+	switch (status) {
+	case MESH_STATUS_SUCCESS: return "Success";
+	case MESH_STATUS_INVALID_ADDRESS: return "Invalid Address";
+	case MESH_STATUS_INVALID_MODEL: return "Invalid Model";
+	case MESH_STATUS_INVALID_APPKEY: return "Invalid AppKey";
+	case MESH_STATUS_INVALID_NETKEY: return "Invalid NetKey";
+	case MESH_STATUS_INSUFF_RESOURCES: return "Insufficient Resources";
+	case MESH_STATUS_IDX_ALREADY_STORED: return "Key Idx Already Stored";
+	case MESH_STATUS_INVALID_PUB_PARAM: return "Invalid Publish Parameters";
+	case MESH_STATUS_NOT_SUB_MOD: return "Not a Subscribe Model";
+	case MESH_STATUS_STORAGE_FAIL: return "Storage Failure";
+	case MESH_STATUS_FEAT_NOT_SUP: return "Feature Not Supported";
+	case MESH_STATUS_CANNOT_UPDATE: return "Cannot Update";
+	case MESH_STATUS_CANNOT_REMOVE: return "Cannot Remove";
+	case MESH_STATUS_CANNOT_BIND: return "Cannot bind";
+	case MESH_STATUS_UNABLE_CHANGE_STATE: return "Unable to change state";
+	case MESH_STATUS_CANNOT_SET: return "Cannot set";
+	case MESH_STATUS_UNSPECIFIED_ERROR: return "Unspecified error";
+	case MESH_STATUS_INVALID_BINDING: return "Invalid Binding";
+
+	default: return "Unknown";
+	}
+}
+
+void print_model_pub(uint16_t ele_addr, uint32_t mod_id,
+						struct mesh_publication *pub)
+{
+	rl_printf("\tElement: %4.4x\n", ele_addr);
+	rl_printf("\tPub Addr: %4.4x", pub->u.addr16);
+	if (mod_id > 0xffff0000)
+		rl_printf("\tModel: %8.8x \n", mod_id);
+	else
+		rl_printf("\tModel: %4.4x \n", (uint16_t) (mod_id & 0xffff));
+	rl_printf("\tApp Key Idx: %4.4x", pub->app_idx);
+	rl_printf("\tTTL: %2.2x", pub->ttl);
+}
+
+void swap_u256_bytes(uint8_t *u256)
+{
+	int i;
+
+	/* End-to-End byte reflection of 32 octet buffer */
+	for (i = 0; i < 16; i++) {
+		u256[i] ^= u256[31 - i];
+		u256[31 - i] ^= u256[i];
+		u256[i] ^= u256[31 - i];
+	}
+}