sforkowany z mirror/RS41ng
124 wiersze
4.0 KiB
C
124 wiersze
4.0 KiB
C
|
#include <string.h>
|
||
|
#include <stdbool.h>
|
||
|
#include "ax25.h"
|
||
|
|
||
|
static inline uint16_t ax25_calculate_crc_for_bit(uint16_t crc, bool bit)
|
||
|
{
|
||
|
uint16_t result = crc;
|
||
|
uint16_t temp;
|
||
|
|
||
|
// XOR lsb of CRC with the latest bit
|
||
|
temp = crc ^ (bit ? 1U : 0U);
|
||
|
// Shift 16-bit CRC one bit to the right
|
||
|
result >>= 1U;
|
||
|
|
||
|
// If XOR result from above has lsb set
|
||
|
if (temp & 0x0001U) {
|
||
|
// Shift 16-bit CRC one bit to the right
|
||
|
result ^= 0x8408U;
|
||
|
}
|
||
|
|
||
|
return result;
|
||
|
}
|
||
|
|
||
|
static inline uint16_t ax25_calculate_crc_for_byte(uint16_t crc, uint8_t byte)
|
||
|
{
|
||
|
uint8_t temp = byte;
|
||
|
for (int i = 0; i < 8; i++, byte >>= 1U) {
|
||
|
bool bit = ((temp & 1U) == 1U) ? true : false;
|
||
|
crc = ax25_calculate_crc_for_bit(crc, bit);
|
||
|
}
|
||
|
|
||
|
return crc;
|
||
|
}
|
||
|
|
||
|
static uint16_t ax25_calculate_crc(size_t length, uint8_t *data)
|
||
|
{
|
||
|
uint16_t crc = 0xFFFF;
|
||
|
|
||
|
for (size_t i = 0; i < length; i++) {
|
||
|
uint8_t byte = data[i];
|
||
|
crc = ax25_calculate_crc_for_byte(crc, byte);
|
||
|
}
|
||
|
|
||
|
return crc;
|
||
|
}
|
||
|
|
||
|
static size_t ax25_encode_digipeater_path(char *input, char *packet_data)
|
||
|
{
|
||
|
size_t digipeaters_length = (size_t) strlen(input);
|
||
|
size_t packet_data_index = 0;
|
||
|
|
||
|
for (size_t index = 0; index < digipeaters_length; index++) {
|
||
|
if (input[index] == ',' || index == digipeaters_length - 1) {
|
||
|
if (input[index] != ',') {
|
||
|
packet_data[packet_data_index] = input[index] == '-' ? ' ' : input[index];
|
||
|
packet_data_index++;
|
||
|
}
|
||
|
|
||
|
size_t fill_count = 7 - (packet_data_index % 7);
|
||
|
while (fill_count > 0 && fill_count < 7) {
|
||
|
packet_data[packet_data_index] = ' ';
|
||
|
fill_count--;
|
||
|
packet_data_index++;
|
||
|
}
|
||
|
|
||
|
continue;
|
||
|
}
|
||
|
|
||
|
packet_data[packet_data_index] = input[index] == '-' ? ' ' : input[index];
|
||
|
packet_data_index++;
|
||
|
}
|
||
|
|
||
|
return packet_data_index;
|
||
|
}
|
||
|
|
||
|
size_t ax25_encode_packet_aprs(char *source, uint8_t source_ssid, char *destination, uint8_t destination_ssid,
|
||
|
char *digipeater_addresses, char *information_field, size_t length, uint8_t *packet_data)
|
||
|
{
|
||
|
// TODO: use length to limit packet size
|
||
|
|
||
|
ax25_packet_header *header = (ax25_packet_header *) packet_data;
|
||
|
|
||
|
header->flag = AX25_PACKET_FLAG;
|
||
|
|
||
|
memset(header->source, ' ', sizeof(header->source));
|
||
|
memcpy(header->source, source, 6);
|
||
|
header->source_ssid = (uint8_t) (source_ssid > '@' ? source_ssid - 6 : source_ssid);;
|
||
|
|
||
|
memset(header->destination, ' ', sizeof(header->destination));
|
||
|
memcpy(header->destination, destination, 6);
|
||
|
header->destination_ssid = destination_ssid;
|
||
|
|
||
|
char *digipeater_addresses_start = ((char *) header) + 1 + 14;
|
||
|
size_t digipeater_addresses_length = ax25_encode_digipeater_path(digipeater_addresses, digipeater_addresses_start);
|
||
|
|
||
|
// Perform bit-shifting for all addresses
|
||
|
uint8_t *actual_data_start = ((uint8_t *) header) + 1;
|
||
|
for (int i = 0; i < 14 + digipeater_addresses_length; i++) {
|
||
|
actual_data_start[i] = actual_data_start[i] << 1U;
|
||
|
}
|
||
|
actual_data_start[13 + digipeater_addresses_length] |= 1U;
|
||
|
|
||
|
ax25_packet_header_end *header_end = (ax25_packet_header_end *) (((uint8_t *) header) + 1 + 14 +
|
||
|
digipeater_addresses_length);
|
||
|
|
||
|
header_end->control_field = AX25_CONTROL_FIELD_UI_FRAME;
|
||
|
header_end->protocol_id = AX25_PROTOCOL_ID_NO_LAYER_3;
|
||
|
|
||
|
size_t info_length = strlen(information_field);
|
||
|
strcpy(header_end->information_field, information_field);
|
||
|
|
||
|
uint16_t crc = ax25_calculate_crc(14 + digipeater_addresses_length + 2 + info_length, actual_data_start);
|
||
|
|
||
|
ax25_packet_footer *footer = (ax25_packet_footer *) (((uint8_t *) header_end->information_field) + info_length);
|
||
|
|
||
|
// CRC is stored MSB first
|
||
|
footer->frame_check_sequence[0] = (crc & 0xFFU) ^ 0xFFU;
|
||
|
footer->frame_check_sequence[1] = (crc >> 8U) ^ 0xFFU;
|
||
|
footer->flag = AX25_PACKET_FLAG;
|
||
|
|
||
|
return 1 + 14 + digipeater_addresses_length + 2 + info_length + 2 + 1;
|
||
|
}
|
||
|
|