net: lwm2m: Fix Security and Server object instance matching

A proper way to match a Security object instance with a Server object
instance is via Short Server ID resource. Both coupled object instances
should carry the same value of this resource in order to me considered
matched.

This was not implemented in the LwM2M library and it was incorrectly
assumed that the Security object instance index corresponds to the
Server object instance index. While such apporach works is simple
scenario, it might yield incorrect results when bootstrap is used.

Fix this, by verifyng the Short Server ID resource in the Secuirty
instance used, and finding a matching Server instance. The server object
instance is stored for future use in the engine.

Additionally, remove an extra Server object instance that was created
when the bootstrap procedure was used. Since the boostrap Security
object instance does not have the corresponding Server object, it's
enough to have a single Server instance.

Signed-off-by: Robert Lubos <robert.lubos@nordicsemi.no>
diff --git a/include/net/lwm2m.h b/include/net/lwm2m.h
index 10bce64..1bd2ce1 100644
--- a/include/net/lwm2m.h
+++ b/include/net/lwm2m.h
@@ -93,6 +93,9 @@
 	/** Current index of Security Object used for server credentials */
 	int sec_obj_inst;
 
+	/** Current index of Server Object used in this context. */
+	int srv_obj_inst;
+
 	/** Flag to enable BOOTSTRAP interface.  See Section 5.2
 	 *  "Bootstrap Interface" of LwM2M Technical Specification 1.0.2
 	 *  for more information.
diff --git a/samples/net/lwm2m_client/src/lwm2m-client.c b/samples/net/lwm2m_client/src/lwm2m-client.c
index 1a867f3..8ca3e83 100644
--- a/samples/net/lwm2m_client/src/lwm2m-client.c
+++ b/samples/net/lwm2m_client/src/lwm2m-client.c
@@ -281,11 +281,14 @@
 	/* Mark 1st instance of security object as a bootstrap server */
 	lwm2m_engine_set_u8("0/0/1", 1);
 
-	/* Create 2nd instance of server and security objects needed for
-	 * bootstrap process
-	 */
+	/* Create 2nd instance of security object needed for bootstrap */
 	lwm2m_engine_create_obj_inst("0/1");
-	lwm2m_engine_create_obj_inst("1/1");
+#else
+	/* Match Security object instance with a Server object instance with
+	 * Short Server ID.
+	 */
+	lwm2m_engine_set_u16("0/0/10", 101);
+	lwm2m_engine_set_u16("1/0/0", 101);
 #endif
 
 	/* setup SERVER object */
@@ -320,6 +323,7 @@
 	/* add power source resource instances */
 	lwm2m_engine_create_res_inst("3/0/6/0");
 	lwm2m_engine_set_res_data("3/0/6/0", &bat_idx, sizeof(bat_idx), 0);
+
 	lwm2m_engine_create_res_inst("3/0/7/0");
 	lwm2m_engine_set_res_data("3/0/7/0", &bat_mv, sizeof(bat_mv), 0);
 	lwm2m_engine_create_res_inst("3/0/8/0");
diff --git a/subsys/net/lib/lwm2m/lwm2m_engine.c b/subsys/net/lib/lwm2m/lwm2m_engine.c
index c0656ea..f7fdf41 100644
--- a/subsys/net/lib/lwm2m/lwm2m_engine.c
+++ b/subsys/net/lib/lwm2m/lwm2m_engine.c
@@ -424,8 +424,8 @@
 	}
 
 	/* defaults from server object */
-	attrs.pmin = lwm2m_server_get_pmin(msg->ctx->sec_obj_inst);
-	attrs.pmax = lwm2m_server_get_pmax(msg->ctx->sec_obj_inst);
+	attrs.pmin = lwm2m_server_get_pmin(msg->ctx->srv_obj_inst);
+	attrs.pmax = lwm2m_server_get_pmax(msg->ctx->srv_obj_inst);
 
 	/* TODO: observe dup checking */
 
@@ -2761,8 +2761,8 @@
 		}
 
 		/* defaults from server object */
-		nattrs.pmin = lwm2m_server_get_pmin(msg->ctx->sec_obj_inst);
-		nattrs.pmax = lwm2m_server_get_pmax(msg->ctx->sec_obj_inst);
+		nattrs.pmin = lwm2m_server_get_pmin(msg->ctx->srv_obj_inst);
+		nattrs.pmax = lwm2m_server_get_pmax(msg->ctx->srv_obj_inst);
 
 		ret = update_attrs(obj, &nattrs);
 		if (ret < 0) {
diff --git a/subsys/net/lib/lwm2m/lwm2m_engine.h b/subsys/net/lib/lwm2m/lwm2m_engine.h
index 150f57c..3aa32b7 100644
--- a/subsys/net/lib/lwm2m/lwm2m_engine.h
+++ b/subsys/net/lib/lwm2m/lwm2m_engine.h
@@ -106,6 +106,7 @@
 
 int32_t lwm2m_server_get_pmin(uint16_t obj_inst_id);
 int32_t lwm2m_server_get_pmax(uint16_t obj_inst_id);
+int lwm2m_server_short_id_to_inst(uint16_t short_id);
 
 #if defined(CONFIG_LWM2M_FIRMWARE_UPDATE_OBJ_SUPPORT)
 uint8_t lwm2m_firmware_get_update_state(void);
diff --git a/subsys/net/lib/lwm2m/lwm2m_obj_server.c b/subsys/net/lib/lwm2m/lwm2m_obj_server.c
index 2bfc9c5..9d84321 100644
--- a/subsys/net/lib/lwm2m/lwm2m_obj_server.c
+++ b/subsys/net/lib/lwm2m/lwm2m_obj_server.c
@@ -133,6 +133,19 @@
 				       CONFIG_LWM2M_SERVER_DEFAULT_PMAX);
 }
 
+int lwm2m_server_short_id_to_inst(uint16_t short_id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(inst); i++) {
+		if (inst[i].obj && server_id[i] == short_id) {
+			return inst[i].obj_inst_id;
+		}
+	}
+
+	return -ENOENT;
+}
+
 static struct lwm2m_engine_obj_inst *server_create(uint16_t obj_inst_id)
 {
 	int index, i = 0, j = 0;
diff --git a/subsys/net/lib/lwm2m/lwm2m_rd_client.c b/subsys/net/lib/lwm2m/lwm2m_rd_client.c
index 21bc7c3..d0599b2 100644
--- a/subsys/net/lib/lwm2m/lwm2m_rd_client.c
+++ b/subsys/net/lib/lwm2m/lwm2m_rd_client.c
@@ -403,72 +403,93 @@
 	sm_handle_timeout_state(msg, ENGINE_INIT);
 }
 
-static int sm_select_next_sec_inst(bool bootstrap_server,
-				   int *sec_obj_inst, uint32_t *lifetime)
+static bool sm_bootstrap_verify(bool bootstrap_server, int sec_obj_inst)
 {
 	char pathstr[MAX_RESOURCE_LEN];
-	int ret, end, i, obj_inst_id, found = -1;
-	bool temp;
+	bool bootstrap;
+	int ret;
+
+	snprintk(pathstr, sizeof(pathstr), "0/%d/1", sec_obj_inst);
+	ret = lwm2m_engine_get_bool(pathstr, &bootstrap);
+	if (ret < 0) {
+		LOG_WRN("Failed to check bootstrap, err %d", ret);
+		return false;
+	}
+
+	if (bootstrap == bootstrap_server) {
+		return true;
+	} else {
+		return false;
+	}
+}
+
+static void sm_update_lifetime(int srv_obj_inst, uint32_t *lifetime)
+{
+	char pathstr[MAX_RESOURCE_LEN];
+
+	snprintk(pathstr, sizeof(pathstr), "1/%d/1", srv_obj_inst);
+	if (lwm2m_engine_get_u32(pathstr, lifetime) < 0) {
+		*lifetime = CONFIG_LWM2M_ENGINE_DEFAULT_LIFETIME;
+		LOG_INF("Using default lifetime: %u", *lifetime);
+	}
+}
+
+static int sm_select_server_inst(int sec_obj_inst, int *srv_obj_inst,
+				 uint32_t *lifetime)
+{
+	char pathstr[MAX_RESOURCE_LEN];
+	uint16_t server_id;
+	int ret, obj_inst_id;
+
+	snprintk(pathstr, sizeof(pathstr), "0/%d/10", sec_obj_inst);
+	ret = lwm2m_engine_get_u16(pathstr, &server_id);
+	if (ret < 0) {
+		LOG_WRN("Failed to obtain Short Server ID, err %d", ret);
+		return -EINVAL;
+	}
+
+	obj_inst_id = lwm2m_server_short_id_to_inst(server_id);
+	if (obj_inst_id < 0) {
+		LOG_WRN("Failed to obtain Server Object instance, err %d",
+			obj_inst_id);
+		return -EINVAL;
+	}
+
+	*srv_obj_inst = obj_inst_id;
+	sm_update_lifetime(*srv_obj_inst, lifetime);
+
+	return 0;
+}
+
+static int sm_select_security_inst(bool bootstrap_server, int *sec_obj_inst)
+{
+	int i, obj_inst_id = -1;
 
 	/* lookup existing index */
 	i = lwm2m_security_inst_id_to_index(*sec_obj_inst);
-	if (i < 0) {
-		*sec_obj_inst = -1;
-		i = -1;
+	if (i >= 0 && sm_bootstrap_verify(bootstrap_server, *sec_obj_inst)) {
+		return 0;
 	}
 
-	/* store end marker, due to looping */
-	end = (i == -1 ? CONFIG_LWM2M_SECURITY_INSTANCE_COUNT : i);
+	*sec_obj_inst = -1;
 
-	/* loop through servers starting from the index after the current one */
-	for (i++; i < end; i++) {
-		if (i >= CONFIG_LWM2M_SECURITY_INSTANCE_COUNT) {
-			i = 0;
-		}
-
+	/* Iterate over all instances to find the correct one. */
+	for (i = 0; i < CONFIG_LWM2M_SECURITY_INSTANCE_COUNT; i++) {
 		obj_inst_id = lwm2m_security_index_to_inst_id(i);
 		if (obj_inst_id < 0) {
+			LOG_WRN("Failed to get inst id for %d", i);
 			continue;
 		}
 
-		/* Query for bootstrap support */
-		snprintk(pathstr, sizeof(pathstr), "0/%d/1",
-			 obj_inst_id);
-		ret = lwm2m_engine_get_bool(pathstr, &temp);
-		if (ret < 0) {
-			continue;
-		}
-
-#if defined(CONFIG_LWM2M_RD_CLIENT_SUPPORT_BOOTSTRAP)
-		if (temp == bootstrap_server) {
-#else
-		if (temp == false) {
-#endif
-			found = obj_inst_id;
-			break;
+		if (sm_bootstrap_verify(bootstrap_server, obj_inst_id)) {
+			*sec_obj_inst = obj_inst_id;
+			return 0;
 		}
 	}
 
-	if (found > -1) {
-		*sec_obj_inst = found;
+	LOG_WRN("sec_obj_inst: No matching servers found.");
 
-		/* query the lifetime */
-		/* TODO: use Short Server ID to link to server info */
-		snprintk(pathstr, sizeof(pathstr), "1/%d/1",
-			 obj_inst_id);
-		if (lwm2m_engine_get_u32(pathstr, lifetime) < 0) {
-			*lifetime = CONFIG_LWM2M_ENGINE_DEFAULT_LIFETIME;
-			LOG_DBG("Using default lifetime: %u", *lifetime);
-		}
-	}
-
-	if (*sec_obj_inst < 0) {
-		/* no servers found */
-		LOG_WRN("sec_obj_inst: No matching servers found.");
-		return -ENOENT;
-	}
-
-	return 0;
+	return -ENOENT;
 }
 
 /* state machine step functions */
@@ -476,6 +497,7 @@
 static int sm_do_init(void)
 {
 	client.ctx->sec_obj_inst = -1;
+	client.ctx->srv_obj_inst = -1;
 	client.trigger_update = 0U;
 	client.lifetime = 0U;
 
@@ -500,9 +522,8 @@
 	}
 
 	client.ctx->bootstrap_mode = true;
-	ret = sm_select_next_sec_inst(client.ctx->bootstrap_mode,
-				      &client.ctx->sec_obj_inst,
-				      &client.lifetime);
+	ret = sm_select_security_inst(client.ctx->bootstrap_mode,
+				      &client.ctx->sec_obj_inst);
 	if (ret < 0) {
 		/* no bootstrap server found, let's move to registration */
 		LOG_WRN("Bootstrap server not found! Try normal registration.");
@@ -510,10 +531,6 @@
 		return ret;
 	}
 
-	if (client.lifetime == 0U) {
-		client.lifetime = CONFIG_LWM2M_ENGINE_DEFAULT_LIFETIME;
-	}
-
 	LOG_INF("Bootstrap started with endpoint '%s' with client lifetime %d",
 		log_strdup(client.ep_name), client.lifetime);
 
@@ -707,17 +724,21 @@
 	}
 
 	client.ctx->bootstrap_mode = false;
-	ret = sm_select_next_sec_inst(client.ctx->bootstrap_mode,
-				      &client.ctx->sec_obj_inst,
-				      &client.lifetime);
+	ret = sm_select_security_inst(client.ctx->bootstrap_mode,
+				      &client.ctx->sec_obj_inst);
 	if (ret < 0) {
 		LOG_ERR("Unable to find a valid security instance.");
 		set_sm_state(ENGINE_INIT);
 		return -EINVAL;
 	}
 
-	if (client.lifetime == 0U) {
-		client.lifetime = CONFIG_LWM2M_ENGINE_DEFAULT_LIFETIME;
+	ret = sm_select_server_inst(client.ctx->sec_obj_inst,
+				    &client.ctx->srv_obj_inst,
+				    &client.lifetime);
+	if (ret < 0) {
+		LOG_ERR("Unable to find a valid server instance.");
+		set_sm_state(ENGINE_INIT);
+		return -EINVAL;
 	}
 
 	LOG_INF("RD Client started with endpoint '%s' with client lifetime %d",
@@ -920,6 +941,7 @@
 {
 	return lwm2m_engine_add_service(lwm2m_rd_client_service,
 					STATE_MACHINE_UPDATE_INTERVAL_MS);
+
 }
 
 SYS_INIT(lwm2m_rd_client_init, APPLICATION,