usb: pd: Increment MessageID only on successful TX

According to the PD 2.0 Spec, Section 6.2.1.3, the MessageID
field shall only be incremented upon successful transmission
(meaning GoodCRC was received). Thus, increment the counter
only when pd_phy_write() succeeds.

Change-Id: I73dce5736c473a1f79db9be868442c7941d13c44
Signed-off-by: Jack Pham <jackp@codeaurora.org>