net: lwm2m: fix sending packet w/o token setup when error

Token is missing when we jump to the error and token is not yet setup.
To correct it, we grab the token from the input packet at the beginning
of the handle_request()

Signed-off-by: Robert Chou <robert.ch.chou@acer.com>
diff --git a/subsys/net/lib/lwm2m/lwm2m_engine.c b/subsys/net/lib/lwm2m/lwm2m_engine.c
index 6c80561..f0b3ad3 100644
--- a/subsys/net/lib/lwm2m/lwm2m_engine.c
+++ b/subsys/net/lib/lwm2m/lwm2m_engine.c
@@ -2421,6 +2421,13 @@
 
 	code = coap_header_get_code(in.in_cpkt);
 
+	/* setup response token */
+	tkl = coap_header_get_token(in.in_cpkt, token);
+	if (tkl) {
+		msg->tkl = tkl;
+		msg->token = token;
+	}
+
 	/* parse the URL path into components */
 	r = coap_find_options(in.in_cpkt, COAP_OPTION_URI_PATH, options, 4);
 	if (r <= 0) {
@@ -2529,13 +2536,6 @@
 		break;
 	}
 
-	/* setup response token */
-	tkl = coap_header_get_token(in.in_cpkt, token);
-	if (tkl) {
-		msg->tkl = tkl;
-		msg->token = token;
-	}
-
 	/* setup incoming data */
 	in.frag = coap_packet_get_payload(in.in_cpkt, &in.offset,
 					  &in.payload_len);