109 lines
3.5 KiB
C
109 lines
3.5 KiB
C
#include "hdlc_frame.h"
|
|
|
|
#include <string.h>
|
|
|
|
#define START_FLAG 0x7E
|
|
#define END_FLAG 0x7E
|
|
#define ESCAPE_FLAG 0x7D
|
|
#define ESCAPE_XOR 0x20
|
|
|
|
void init_HDLCFrame(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(const HDLCFrame *frame, size_t i) {
|
|
uint16_t fcs = 0xFFFF;
|
|
|
|
uint8_t data_bytes[2 + frame->data_length];
|
|
data_bytes[0] = frame->address;
|
|
data_bytes[1] = frame->control;
|
|
memcpy(data_bytes + 2, frame->data, frame->data_length);
|
|
|
|
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;
|
|
}
|
|
|
|
return fcs;
|
|
}
|
|
|
|
void create_frame(const HDLCFrame* frame, uint8_t* result) {
|
|
size_t index = 0;
|
|
|
|
result[index++] = START_FLAG;
|
|
result[index++] = frame->address;
|
|
result[index++] = frame->control;
|
|
|
|
if (frame->data != NULL) {
|
|
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) {
|
|
result[index++] = ESCAPE_FLAG;
|
|
result[index++] = byte ^ ESCAPE_XOR;
|
|
} else {
|
|
result[index++] = byte;
|
|
}
|
|
}
|
|
}
|
|
|
|
uint16_t fcs = calculate_fcs(frame, 0);
|
|
result[index++] = fcs & 0xFF;
|
|
result[index++] = (fcs >> 8) & 0xFF;
|
|
result[index++] = END_FLAG;
|
|
}
|
|
|
|
void init_IFrame(IFrame* frame, uint8_t address, uint8_t receive_sequence_number, uint8_t poll_final,
|
|
uint8_t send_sequence_number, uint8_t* data, size_t data_length) {
|
|
init_HDLCFrame(&frame->base, address,
|
|
((receive_sequence_number & 0b111) << 6) | ((poll_final & 0b1) << 4) |
|
|
((send_sequence_number & 0b111) << 1) | 0,
|
|
data, data_length);
|
|
frame->receive_sequence_number = receive_sequence_number;
|
|
frame->poll_final = poll_final;
|
|
frame->send_sequence_number = send_sequence_number;
|
|
}
|
|
|
|
void init_SFrame(SFrame* frame, uint8_t address, uint8_t receive_sequence_number, uint8_t poll_final,
|
|
const char* frame_type) {
|
|
uint8_t frame_type_value;
|
|
if (strcmp(frame_type, "RR") == 0) {
|
|
frame_type_value = 0;
|
|
} else if (strcmp(frame_type, "RNR") == 0) {
|
|
frame_type_value = 1;
|
|
} else if (strcmp(frame_type, "REJ") == 0) {
|
|
frame_type_value = 2;
|
|
} else if (strcmp(frame_type, "SREJ") == 0) {
|
|
frame_type_value = 3;
|
|
} else {
|
|
// Handle error
|
|
return;
|
|
}
|
|
|
|
init_HDLCFrame(&frame->base, address,
|
|
((receive_sequence_number & 0b111) << 6) | ((poll_final & 0b1) << 5) |
|
|
((frame_type_value & 0b111) << 2) | 1,
|
|
NULL, 0);
|
|
frame->receive_sequence_number = receive_sequence_number;
|
|
frame->poll_final = poll_final;
|
|
frame->frame_type = frame_type;
|
|
}
|
|
|
|
void init_UFrame(UFrame* frame, uint8_t address, uint8_t poll_final, const char* frame_type,
|
|
uint8_t* data, size_t data_length) {
|
|
uint8_t frame_type_value;
|
|
if (strcmp(frame_type, "BP") == 0) {
|
|
frame_type_value = 63;
|
|
} else {
|
|
// Handle error
|
|
return;
|
|
}
|
|
|
|
init_HDLCFrame(&frame->base, address, (frame_type_value << 2) | 3, data, data_length);
|
|
frame->poll_final = poll_final;
|
|
frame->frame_type = frame_type;
|
|
}
|