diag: Prevent out-of-bound read while processing peripheral ctrl_pkt

There is a possibility of out-of-bound access while processing control
packet received from peripheral due to missing buffer length check.
The patch adds proper check to fix the same.

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