91 lines
1.9 KiB
C
91 lines
1.9 KiB
C
//**************************************************************************************
|
|
/*! \copyright: 2020-2021 Thales Alenia Space Deutschland GmbH
|
|
* \project: multiMIND
|
|
* \file: (name of source file: hdlc.c)
|
|
* \date: (09.02.2022)
|
|
* \author: (Stelios Filippopoulos)
|
|
* \brief: (hdlc functions)
|
|
* \language: (C)
|
|
**************************************************************************************
|
|
*/
|
|
|
|
#include "tas/hdlc.h"
|
|
#include "tas/crc.h"
|
|
|
|
#include <stdint.h>
|
|
|
|
void hdlc_add_byte(uint8_t ch, uint8_t *buff, size_t *pos)
|
|
{
|
|
size_t templen = *pos;
|
|
|
|
if ((ch == 0x7E) ||
|
|
(ch == 0x7D) ||
|
|
(ch == 0x7C))
|
|
{
|
|
buff[templen++] = 0x7D;
|
|
ch ^= 0x20;
|
|
}
|
|
buff[templen++] = ch;
|
|
|
|
*pos = templen;
|
|
}
|
|
|
|
void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen)
|
|
{
|
|
size_t tlen = 0;
|
|
uint16_t ii;
|
|
uint16_t crc16;
|
|
uint8_t bt;
|
|
|
|
// calc crc16
|
|
crc16 = calc_crc16_buff_reflected( src, slen );
|
|
|
|
dst[tlen++] = 0x7E;
|
|
for (ii = 0; ii < slen; ii++)
|
|
{
|
|
bt = *src++;
|
|
hdlc_add_byte(bt, dst, &tlen);
|
|
}
|
|
|
|
// hdlc crc16 is in little endian format
|
|
// WARNING: This is not portable code! Bytes need to be swapped on a big
|
|
// endian system
|
|
// TODO: Fix
|
|
hdlc_add_byte((uint8_t) (crc16 & 0xFF), dst, &tlen);
|
|
hdlc_add_byte((uint8_t) ((crc16 >> 8) & 0xFF), dst, &tlen);
|
|
|
|
dst[tlen++] = 0x7C;
|
|
*dlen = tlen;
|
|
}
|
|
|
|
int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen)
|
|
{
|
|
uint16_t tlen = 0;
|
|
uint16_t ii;
|
|
uint8_t bt;
|
|
|
|
*dlen = 0;
|
|
if (slen < 4) return -1;
|
|
if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return -2;
|
|
src++;
|
|
for (ii = 1; ii < slen-1; ii++)
|
|
{
|
|
bt = *src++;
|
|
|
|
if (bt == 0x7D)
|
|
{
|
|
bt = *src++ ^ 0x20;
|
|
ii++;
|
|
}
|
|
dst[tlen++] = bt;
|
|
}
|
|
// calc crc16
|
|
if(calc_crc16_buff_reflected( dst, tlen ) != 0x0f47) {
|
|
return 1;
|
|
}
|
|
*dlen = tlen - 2;
|
|
return 0;
|
|
}
|
|
|
|
|