diag: Add proper check for peripheral value

Currently, due to improper check for peripheral
value, wrong index of diag feature array might
be accessed. The patch fixes the issue by adding
correct check.

Change-Id: I274f08889a9bc99763e6a57cc51172e20488c0cf
Signed-off-by: Mohit Aggarwal <maggarwa@codeaurora.org>