用CAPL实现S19文件解析

最近一直在忙一个项目，需要在CAPL里面读取S19文件。
S19文件可以直接用记事本打开，经过分析文件格式，在CAPL里面按字符解析，可以得到S19的所有数据，目前仅仅支持S2和S3格式
参考代码如下：

variables
{
  byte dataBuffer[0x0FFFFF];//memory size
}

void Read_s19File(char Filename[])
{
  long file_handle;
  char str[100];
  long char_index=0;
  long mem_Address;
  int Line_Length;
  long buff_index=0; 
  long length;
  file_handle=OpenFileRead(Filename,0);
  if(file_handle!=0)
  {
    buff_index=0;
    // Read all lines
    while ( fileGetString(str,elcount(str),file_handle)!=0 )
    {
      if((str[0]=='S')&&(str[1]=='3'))
      {
        /*memory address char 4 to 11*/
        mem_Address= char2byte(str[4])*0x10000000+char2byte(str[5])*0x1000000
                      +char2byte(str[6])*0x100000+char2byte(str[7])*0x10000
                      +char2byte(str[8])*0x1000+char2byte(str[9])*0x100
                      +char2byte(str[10])*0x10+char2byte(str[11]);
        // the number of bytes each line contains
        Line_Length = (char2byte(str[2])*0x10+char2byte(str[3]));
        for(char_index=0; char_index<(2*(Line_Length-5)); char_index=char_index+2)
        {
          dataBuffer[buff_index++]=(char2byte(str[char_index+12])*0x10+char2byte(str[char_index+13]));
        }
      }
      else  if((str[0]=='S')&&(str[1]=='2'))
      {
        /*memory address char 4 to 9*/
        mem_Address= char2byte(str[4])*0x100000+char2byte(str[5])*0x10000
                      +char2byte(str[6])*0x1000+char2byte(str[7])*0x100
                      +char2byte(str[8])*0x10+char2byte(str[9]);
        // the number of bytes each line contains
        Line_Length = (char2byte(str[2])*0x10+char2byte(str[3]));
        for(char_index=0; char_index<(2*(Line_Length-4)); char_index=char_index+2)
        {
          dataBuffer[buff_index++]=(char2byte(str[char_index+10])*0x10+char2byte(str[char_index+11]));
        }
      }
    }
  }
  fileClose(file_handle);
}

byte char2byte(char ch)
{
   byte  val = 0;
   if ( ch >= '0' && ch <= '9')
   {
      val = ch - '0';      
   }
   if ( ch >= 'a' && ch <= 'f')
   {
      val = (ch - 'a') + 10;      
   }
   if ( ch >= 'A' && ch <= 'F')
   {
      val = (ch - 'A') + 10;       
   }
   return val;
}