diag: Prevent out-of-bound access while processing stm cmd

Proper buffer length check is missing before processing
stm cmd, which can lead to out-of-bound access. The patch
adds proper check for the same.

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