add new protocl controller for ag1010

r3673 | jgn | 2012-07-23 13:41:34 +1000 (Mon, 23 Jul 2012) | 1 line

site_ansto/hardsup/sct_lfprot.c

@@ -0,0 +1,213 @@
+/**
+* @brief Protocol handler for LG AG1010 Power generator/supplier
+*
+* The protocol is to handle the Hexidecimal strings sending and receiving from the device,
+* the protocol also calculates the CRC for each data sum sent out
+*
+* Author: Jing Chen, jgn@ansto.gov.au
+*
+******************************************************************************************/
+
+#include <errno.h>
+#include <ascon.h>
+#include <ascon.i>
+#include <dynstring.h>
+
+static int dbgprintf(char* fmtstr, ...);
+
+typedef unsigned char BYTE;
+
+BYTE updateCRC8(BYTE data, BYTE previousCrc) {
+  // Polynomial: x^8 + x^5 + x^4 + 1
+  BYTE crc = previousCrc;
+  int b;
+  for(b = 0; b < 8; b++) {
+    if((crc ^ data) & 0x01) {
+      crc = crc ^ 0x18;
+      crc = crc >> 1;
+      crc = crc | 0x80;
+    } else {
+      crc = crc >> 1;
+      crc = crc & 0x7f;
+    }
+    data = data >> 1;
+  }
+
+  return crc;
+}
+
+BYTE char_to_int(char c){
+  BYTE tmp;
+  if ('0' <= c && c <= '9') {
+    tmp = c - '0';
+  } else if ('a' <= c && c <= 'f') {
+    tmp = 10 + c - 'a';
+  } else if ('A' <= c && c <= 'F') {
+    tmp = 10 + c - 'A';
+  } else {
+    printf("Error in string.\n");
+    return NULL;
+  }
+
+  return tmp;
+}
+
+BYTE getHexCode(BYTE startPos, char *str){
+
+  BYTE tmp = 0;
+
+  tmp = (0x0F & char_to_int(str[startPos])) << 4;
+  tmp = tmp | (0x0F & char_to_int(str[startPos+1]));
+
+  return tmp;
+}
+
+
+/** @brief Send the Hexdecimal string with CRC 
+*/
+int LFGenWriteStart(Ascon *a) {
+
+  char         *pNewBuffer;
+  unsigned char bufSize, bufInd;
+
+  double now;
+
+  // Get real packet information from the buffer
+  char *curText = GetCharArray(a->wrBuffer);
+  BYTE HEAD, LEN, CTRL, CRC=0;
+  HEAD = getHexCode(0, curText);
+  LEN  = getHexCode(2, curText);
+
+  // buffer size, 
+  pNewBuffer = (char *)malloc((LEN+2)*sizeof(char));
+  bufInd=0;
+  pNewBuffer[bufInd] = HEAD;
+  bufInd++;
+  pNewBuffer[bufInd] = LEN;
+  bufInd++;
+  CRC = updateCRC8(HEAD, CRC);
+  CRC = updateCRC8(LEN, CRC);
+
+  if(LEN<2){
+    a->state = AsconIdle;
+    AsconError(a, "Error Datasum Provided!", 0);
+    return 0;
+  } else {
+    CTRL = getHexCode(4, curText);
+    pNewBuffer[bufInd] = CTRL;
+    bufInd++;
+    CRC = updateCRC8(CTRL, CRC);
+    if(LEN>2){
+      int i;
+      for(i=0; i<LEN-2; i++){
+        pNewBuffer[bufInd] = getHexCode(bufInd*2, curText);
+        CRC = updateCRC8(pNewBuffer[bufInd], CRC);
+        bufInd++;
+      }
+    }
+  }
+
+  pNewBuffer[bufInd] = CRC;
+  bufInd++;
+  //pNewBuffer[bufSize-1] = '\r';
+  DynStringReplaceWithLen(a->wrBuffer, pNewBuffer, 0, bufInd);
+  a->wrPos = 0;
+  a->state = AsconWriting;
+  /* if there is only the terminator, do not send it */
+  if (GetDynStringLength(a->wrBuffer) <= 1) {
+    a->state = AsconWriteDone;
+  }
+  //dbgprintf("HEAD=%02x; LEN=%02x; CTRL=%02x; DATA=%s; CRC=%2x -> %s\n", HEAD, LEN, CTRL, Data, CRC, pNewBuffer);
+  free(pNewBuffer);
+  return 1;
+}
+
+
+int LFConcatChar(pDynString self, char chr){
+
+  int  ret;
+  char *lfChr[2];
+
+  BYTE  lfByte = (BYTE)chr;
+  sprintf(lfChr, "%02x", lfByte); 
+  DynStringConcat(self, lfChr);
+  
+  return 1;
+}
+
+int LFGenReading(Ascon *a)
+{
+  int  ret;
+  char chr=0;
+  BYTE *lfChr;
+  //BYTE HEAD, LEN, CTRL, CRC=0, *Data=NULL;
+
+  DynStringClear(a->rdBuffer);
+  
+  ret = AsconReadChar(a->fd, &chr);
+  if (chr == 0) {
+    if (a->timeout > 0) {
+      if (DoubleTime() - a->start > a->timeout) {
+        AsconError(a, "read timeout", 0);
+        a->state = AsconTimeout;
+      }
+    }
+    return 1;
+  }
+
+  LFConcatChar(a->rdBuffer, chr);
+
+  while ((ret = AsconReadChar(a->fd, &chr)) > 0) {
+    LFConcatChar(a->rdBuffer, chr);
+  }
+  a->state = AsconReadDone;
+  //DynStringConcatChar(a->rdBuffer, '\0');
+}
+
+int LFGenProtHandler(Ascon *a)
+{
+  int ret;
+
+  switch(a->state){
+    case AsconWriteStart:
+      ret = LFGenWriteStart(a);
+      return ret;
+      break;
+    case AsconReading:
+      return LFGenReading(a);
+      break;
+    default:
+      return AsconStdHandler(a);
+  }
+}
+
+void AddLFGenProtocol()
+{
+  AsconProtocol *prot = NULL;
+
+  prot = calloc(sizeof(AsconProtocol), 1);
+  prot->name = strdup("lfgen");
+  prot->init = AsconStdInit;
+  prot->handler = LFGenProtHandler;
+  AsconInsertProtocol(prot);
+}
+
+#include <stdio.h>
+#include <stdarg.h>
+
+static int dbgprintf(char* fmtstr, ...) {
+    FILE* fp = NULL;
+    int ret = 0;
+    fp = fopen("/home/jgn/tmp/lfdbg.txt", "a");
+
+    if (fp != NULL) {
+      va_list ap;
+      va_start(ap, fmtstr);
+      ret = vfprintf(fp, fmtstr, ap);
+      va_end(ap);
+      fclose(fp);
+    }
+    return ret;
+}
+
+