It may be possible the problem is in your crc16 or some other related section. In the code you posted, I do not see any led(did I miss it?)

This code appears Ok to me.