СКС16-CCITT в CoDeSys 2.3

Помогите перевести на язык ST следующий код
Описание в документации прибора:
CRC: The 16-bit CRC checksum. The algorithm used is CRC-CCITT.
The CRC calculation includes all bytes of the frame.
 The data bytes have to be calculated as a word. 
At first you have to shift in the high byte of the data word. 
This is the opposite way you transmit the data word. 
The 16-bit generator polynomial ‘x^16+x^12+x^5+1’ is used for the calculation. 
Order of CRC calculation: ‘OpCode’, ‘len-1’, ‘data[0]’ high byte, ‘data[0]’ low byte, ...
CRC: Checksum of the frame. The low byte is transmitted first.

/*
  Name  : CRC-16 CCITT
  Poly  : 0x1021    x^16 + x^12 + x^5 + 1
  Init  : 0xFFFF
  Revert: false
  XorOut: 0x0000
  Check : 0x29B1 ("123456789")
  MaxLen: 4095 байт (32767 бит) - обнаружение
    одинарных, двойных, тройных и всех нечетных ошибок
*/
unsigned short Crc16(unsigned char *pcBlock, unsigned short len)
{
    unsigned short crc = 0xFFFF;
    unsigned char i;
 
    while (len--)
    {
        crc ^= *pcBlock++ << 8;
 
        for (i = 0; i < 8; i++)
            crc = crc & 0x8000 ? (crc << 1) ^ 0x1021 : crc << 1;
    }
 
    return crc;