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