7E加码解码

//===========================================================================
// 加密单个字符
uint8_t*
hdlc_encode_buf(uint8_t *buf, uint8_t c)
{
  switch (c)
  {
  case 0x7E:
    *buf++ = 0x7D;
    *buf++ = 0x02;
    break;

  case 0x7D:
    *buf++ = 0x7D;
    *buf++ = 0x01;
    break;

  default:
    *buf++ = c;
    break;
  }

  return buf;
}


//===========================================================================
// 加密字符串
static void
encode_process(uint8_t *encoded_buf, uint8_t *data, uint16_t length)
{
  uint16_t i;

  *encoded_buf++ = 0x7E;
  while(length--)
  {
    hdlc_encode_buf(encoded_buf, *data++);
  }
  *encoded_buf++ = 0x7E;
}

static struct pt pt_decode;
static uint8_t buffer[1024] = {0};
static count = 0;

static char
decode_char(uint8_t c)
{
  uint8_t ch;
  struct pt *pt = &pt_decode;

  PT_BEGIN(pt);
  PT_WAIT_UNTIL(pt, c == 0x7E);

  while(1)
  {
    PT_YIELD(pt);

    if(0x7D == c)
    {
      PT_YIELD(pt);
      if(0x01 == c)
      {
        ch = 0x7D;
      }
      else
      {
        ch = 0x7E;
      }
    }
    else
    {
      ch = c;
    }

    buffer[count++] = ch;
  }

  PT_END(pt);
}

static void
decode_process(uint8_t *data, uint16_t length)
{
  while(length--)
  {
    decode_char(*data);
    ++data;
  }
}