78 lines
2.6 KiB
C
78 lines
2.6 KiB
C
#include "hdlc_frame.h"
|
|
#include <stdlib.h>
|
|
|
|
#define START_FLAG 0x7E
|
|
#define END_FLAG 0x7E
|
|
#define ESCAPE_FLAG 0x7D
|
|
#define ESCAPE_XOR 0x20
|
|
|
|
void initialize_frame(HDLCFrame* frame, uint8_t address, uint8_t control, uint8_t* data, size_t data_length) {
|
|
frame->address = address;
|
|
frame->control = control;
|
|
frame->data = data;
|
|
frame->data_length = data_length;
|
|
}
|
|
|
|
uint16_t calculate_fcs(HDLCFrame* frame) {
|
|
uint16_t fcs = 0xFFFF;
|
|
uint8_t* data_bytes = malloc(frame->data_length + 2);
|
|
data_bytes[0] = frame->address;
|
|
data_bytes[1] = frame->control;
|
|
|
|
for (size_t i = 0; i < frame->data_length; i++) {
|
|
data_bytes[i + 2] = frame->data[i];
|
|
}
|
|
|
|
for (size_t i = 0; i < frame->data_length + 2; i++) {
|
|
uint8_t byte = data_bytes[i];
|
|
fcs = (fcs >> 8) ^ (fcs << 8) ^ byte;
|
|
fcs &= 0xFFFF;
|
|
}
|
|
|
|
free(data_bytes);
|
|
return fcs;
|
|
}
|
|
|
|
void create_frame(HDLCFrame* frame, uint8_t** frame_data, size_t* frame_length) {
|
|
*frame_length = 4 + frame->data_length;
|
|
*frame_data = malloc(*frame_length);
|
|
(*frame_data)[0] = START_FLAG;
|
|
(*frame_data)[1] = frame->address;
|
|
(*frame_data)[2] = frame->control;
|
|
|
|
size_t index = 3;
|
|
for (size_t i = 0; i < frame->data_length; i++) {
|
|
uint8_t byte = frame->data[i];
|
|
if (byte == START_FLAG || byte == END_FLAG || byte == ESCAPE_FLAG) {
|
|
(*frame_data)[index++] = ESCAPE_FLAG;
|
|
(*frame_data)[index++] = byte ^ ESCAPE_XOR;
|
|
} else {
|
|
(*frame_data)[index++] = byte;
|
|
}
|
|
}
|
|
|
|
uint16_t fcs = calculate_fcs(frame);
|
|
(*frame_data)[index++] = fcs & 0xFF;
|
|
(*frame_data)[index] = END_FLAG;
|
|
}
|
|
|
|
HDLCFrame* create_u_frame(uint8_t address, uint8_t control, uint8_t* data, size_t data_length) {
|
|
HDLCFrame* frame = malloc(sizeof(HDLCFrame));
|
|
initialize_frame(frame, address, control, data, data_length);
|
|
return frame;
|
|
}
|
|
|
|
HDLCFrame* create_s_frame(uint8_t address, uint8_t control, uint8_t* data, size_t data_length, uint8_t receive_sequence_number, uint8_t send_sequence_number) {
|
|
HDLCFrame* frame = malloc(sizeof(HDLCFrame));
|
|
initialize_frame(frame, address, control, data, data_length);
|
|
frame->control |= (receive_sequence_number << 2) & 0xFC;
|
|
frame->control |= (send_sequence_number << 1) & 0xFE;
|
|
return frame;
|
|
}
|
|
|
|
HDLCFrame* create_i_frame(uint8_t address, uint8_t control, uint8_t* data, size_t data_length, uint8_t sequence_number) {
|
|
HDLCFrame* frame = malloc(sizeof(HDLCFrame));
|
|
initialize_frame(frame, address, control, data, data_length);
|
|
frame->control |= (sequence_number << 1) & 0xFE;
|
|
return frame;
|
|
} |