eive-obsw/thirdparty/tas/hdlc.c
Robin Mueller 02eff4a26f
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
some more bugfixes
2022-11-18 14:18:45 +01:00

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;
}