16位CRC校验

现在正在做CRC校验过和如下
CRC码的计算方法如下： 1．预置1个16位的寄存器（下称CRC寄存器），初试化其内容为十六进制数0xFFFF；
2．把第一个待计算的数据即通讯信息帧的首字节与CRC寄存器的低8位相异或，把结果放回CRC寄存器；
3．把CRC寄存器的内容右移一位用0填补最高位即进行逻辑移位处理并检查右移后的移出位；
4．如果移出位为0重复第3步即再次右移一位，如果移出位为1则CRC寄存器与CRC生成多项式0xA001（1010 0000 0000 0001）进行异或运算，结果放回CRC寄存器；
5．重复步骤3和4，直到右移8次，对整个8位数据全部进行同样处理；
6．重复步骤2到步骤5，进行通讯信息帧下一个字节的处理；
7．将该通讯信息帧所有字节按上述步骤计算完成后，最后得到的CRC寄存器的内容即为CRC校验码，将其附着在原通讯信息帧后进行传输即可。如果用于Modbus协议设备则还需交换高低8位，非Modbus协议设备可以不必交换。接收方依同样方法进行计算，只是计算的内容不要包括最后两个字节，然后比较生成的CRC校验码和数据帧中的是否相同即可。如果发送方交换CRC校验码的高低8位，则接收方可以计算包括交换后的CRC码在内的整个数据帧，传输正确的话结果恒为0x0000。
哪位大哥帮我写个算法