Modbus16 CRC

Hallo Leute,

ich schreibe gerade C-Programm für einen ATMega32 einer Steuerung und 
möchte diese auch via RS232 vom PC aus steuern können. Die 
State-Maschine für die Kommunikation steht soweit, mir fehlt lediglich 
noch eine "normkonforme" CRC. Hierbei möchte ich gern eine Modbus16 CRC 
verwenden, weis jedoch leider nicht, wie ich das angehen soll und leider 
habe ich bis jetzt auch noch nicht wirklich etwas aufschlussreiches im 
I-Net gefunden. Könnte mir dabei event. jemand helfen?

Derzeit sieht mein Code so aus (wie gesagt leider nicht Modbus16 
konform):


#define  GP   0xA001  //generating polynomial
unsigned char get_crc(unsigned char *data, unsigned char todo, unsigned 
char pos)
{    short crc;
     unsigned char c1=0, c2=0;
     unsigned char crcbit;
     unsigned char c, i;

     crc = 0xFFFF;



     for(c=0; c < chFrame_Length-2; c++)
     {
          crc ^= data[c] & 0xFF;
          for(i = 0; i < 8; i++)
          {
               crcbit = crc & 0x0001;
               crc>>=1;
               if(crcbit)
                    crc ^= GP;

          }




     }


     c1 = (crc >> 8);
     c2 = crc;

     if(todo == 1)                // Überprüfen ob CRC stimmt
     {


          if((data[chFrame_Length-1] == c1) && (data[chFrame_Length] == 
c2))
               return 1;

          else
               return 0;
     }

     else if(todo == 0)
     {
          data[pos]   = c1;
          data[pos+1] = c2;

     }

}

LG
Mark