diag: Check for feature mask status before sending any command

Currently, APPS diag may send commands to peripherals even before
forwarding feature mask. This patch adds new check which ensures
that any command is sent to peripheral only after forwarding
feature mask.

Change-Id: I024ad6d3bcb2636f6f7d82a3744f034110ec1eb6
Signed-off-by: Katish Paran <kparan@codeaurora.org>