diag: Fix possible uninitialized and out-of-bound access

There is possibility of uninitialized and out-of-bound
access due to missing checks. The patch fixes the same
with adding proper checks.

Change-Id: I88c7a125e5a05246acf511b9fd1ff0931b676c48
Signed-off-by: Hardik Arya <harya@codeaurora.org>