381 lines
8.2 KiB
C
381 lines
8.2 KiB
C
/* HDLC
|
|
*
|
|
* (C) 2022 by Andreas Eversberg <jolly@eversberg.eu>
|
|
* All Rights Reserved
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include "../libdebug/debug.h"
|
|
#include "uk0.h"
|
|
|
|
//#define DEBUG_HDLC
|
|
|
|
/* calculate CRC from the bits of label and data and 16 zeroes.
|
|
* the result is the remainder of the polynomial division and
|
|
* conforms to HDLC/X25/ISDN standard.
|
|
*/
|
|
static uint16_t crc16(uint8_t *data, int length)
|
|
{
|
|
uint16_t generator = 0x8408;
|
|
uint16_t crc = 0xffff; /* init crc register with 1 */
|
|
int i, j;
|
|
|
|
for (i = 0; i < length; i++) {
|
|
for (j = 0; j < 8; j++) {
|
|
if (((data[i]>>j) & 1) != (crc & 1)) {
|
|
crc = crc >> 1;
|
|
crc = crc ^ generator;
|
|
} else
|
|
crc = crc >> 1;
|
|
}
|
|
}
|
|
|
|
return crc ^ 0xffff;
|
|
}
|
|
|
|
/*
|
|
* TX
|
|
*/
|
|
|
|
void hdlc_tx_init(hdlc_tx_t *tx, struct uk0 *uk0, int channel, enum hdlc_mode mode)
|
|
{
|
|
memset(tx, 0, sizeof(*tx));
|
|
tx->uk0 = uk0;
|
|
tx->channel = channel;
|
|
tx->mode = mode;
|
|
}
|
|
|
|
static inline int tx_bit(hdlc_tx_t *tx, uint8_t *bit)
|
|
{
|
|
int subsequent = 0;
|
|
|
|
if (tx->state == HDLC_STATE_IDLE) {
|
|
/* when index is set, we just had a frame, so we can use end flag as start flag */
|
|
if (tx->index)
|
|
subsequent = 1;
|
|
tx->length = uk0_tx_dequeue(tx->uk0, tx->channel, tx->buffer, sizeof(tx->buffer));
|
|
tx->index = 0;
|
|
if (!tx->length)
|
|
return -1; /* no more data */
|
|
tx->crc = crc16(tx->buffer, tx->length);
|
|
if (subsequent) {
|
|
tx->state = HDLC_STATE_TX_DATA;
|
|
#ifdef DEBUG_HDLC
|
|
printf("subsequent frame, continue with data.\n");
|
|
#endif
|
|
}
|
|
else {
|
|
tx->state = HDLC_STATE_TX_START_FLAG;
|
|
#ifdef DEBUG_HDLC
|
|
printf("new frame after pause, continue with flag.\n");
|
|
#endif
|
|
}
|
|
tx->bits = 0;
|
|
tx->frame = 0x00;
|
|
}
|
|
|
|
switch (tx->state) {
|
|
case HDLC_STATE_TX_START_FLAG:
|
|
*bit = (0x7e >> tx->bits) & 1;
|
|
#ifdef DEBUG_HDLC
|
|
printf("transmit bit: %d\n", *bit);
|
|
#endif
|
|
if (++tx->bits == 8) {
|
|
tx->bits = 0;
|
|
#ifdef DEBUG_HDLC
|
|
printf("flag sent, continue with data.\n");
|
|
#endif
|
|
tx->state = HDLC_STATE_TX_DATA;
|
|
}
|
|
break;
|
|
case HDLC_STATE_TX_DATA:
|
|
if ((tx->frame & 0x1f) == 0x1f) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("insert stuffing bit.\n");
|
|
#endif
|
|
*bit = 0;
|
|
#ifdef DEBUG_HDLC
|
|
printf("transmit bit: %d\n", *bit);
|
|
#endif
|
|
tx->frame <<= 1;
|
|
break;
|
|
}
|
|
*bit = (tx->buffer[tx->index] >> tx->bits) & 1;
|
|
#ifdef DEBUG_HDLC
|
|
printf("transmit bit: %d\n", *bit);
|
|
#endif
|
|
tx->frame <<= 1;
|
|
tx->frame |= *bit;
|
|
if (++tx->bits == 8) {
|
|
tx->bits = 0;
|
|
if (++tx->index == tx->length) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("data complete, continue with crc.\n");
|
|
#endif
|
|
tx->state = HDLC_STATE_TX_CRC;
|
|
}
|
|
}
|
|
break;
|
|
case HDLC_STATE_TX_CRC:
|
|
if ((tx->frame & 0x1f) == 0x1f) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("insert stuffing bit.\n");
|
|
#endif
|
|
*bit = 0;
|
|
#ifdef DEBUG_HDLC
|
|
printf("transmit bit: %d\n", *bit);
|
|
#endif
|
|
tx->frame <<= 1;
|
|
break;
|
|
}
|
|
*bit = (tx->crc >> tx->bits) & 1;
|
|
#ifdef DEBUG_HDLC
|
|
printf("transmit bit: %d\n", *bit);
|
|
#endif
|
|
tx->frame <<= 1;
|
|
tx->frame |= *bit;
|
|
if (++tx->bits == 16) {
|
|
tx->bits = 0;
|
|
#ifdef DEBUG_HDLC
|
|
printf("data complete, continue with end flag.\n");
|
|
#endif
|
|
tx->state = HDLC_STATE_TX_END_FLAG;
|
|
}
|
|
break;
|
|
case HDLC_STATE_TX_END_FLAG:
|
|
*bit = (0x7e >> tx->bits) & 1;
|
|
#ifdef DEBUG_HDLC
|
|
printf("transmit bit: %d\n", *bit);
|
|
#endif
|
|
if (++tx->bits == 8) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("end flag complete, enter idle state for next frame.\n");
|
|
#endif
|
|
tx->state = HDLC_STATE_IDLE;
|
|
}
|
|
break;
|
|
default:
|
|
return -1; /* should never happen */
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* data requested from uk0 coming from tx_queue */
|
|
void uk0_send(uk0_t *uk0, int ch, uint8_t *data, int length)
|
|
{
|
|
hdlc_tx_t *tx;
|
|
int i, j, rc;
|
|
uint8_t bit;
|
|
|
|
if (ch < 1 || ch > 3) {
|
|
fprintf(stderr, "%s: illegal channel number %d!\n", __func__, ch);
|
|
return;
|
|
}
|
|
tx = &uk0->hdlc_tx[ch - 1];
|
|
|
|
/* disabled -> transparent */
|
|
switch (tx->mode) {
|
|
case HDLC_MODE_OFF:
|
|
memset(data, 0xff, length);
|
|
break;
|
|
case HDLC_MODE_TRANS:
|
|
while (length) {
|
|
if (tx->index == tx->length) {
|
|
tx->length = uk0_tx_dequeue(tx->uk0, tx->channel, tx->buffer, sizeof(tx->buffer));
|
|
tx->index = 0;
|
|
}
|
|
if (!tx->length) {
|
|
memset(data, 0xff, length);
|
|
break;
|
|
}
|
|
while (length && tx->index < tx->length) {
|
|
*data++ = tx->buffer[tx->index];
|
|
tx->index++;
|
|
length--;
|
|
}
|
|
}
|
|
break;
|
|
case HDLC_MODE_HDLC:
|
|
for (i = 0; i < length; i++) {
|
|
for (j = 0; j < 8; j++) {
|
|
rc = tx_bit(tx, &bit);
|
|
/* rc is set, if no more HDLC frames */
|
|
if (rc)
|
|
break;
|
|
data[i] = (data[i] >> 1) | (bit << 7);
|
|
}
|
|
/* fill up byte, if no more HDLC frames */
|
|
if (j < 8) {
|
|
data[i] = (data[i] >> (8 - j)) | (0xff << j);
|
|
i++;
|
|
break;
|
|
}
|
|
}
|
|
/* fill up remaining data with 0xff, if no more HDLC frames */
|
|
if (i < length)
|
|
memset(data + i , 0xff, length - i);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* RX
|
|
*/
|
|
|
|
void hdlc_rx_init(hdlc_rx_t *rx, struct uk0 *uk0, int channel, enum hdlc_mode mode)
|
|
{
|
|
memset(rx, 0, sizeof(*rx));
|
|
rx->uk0 = uk0;
|
|
rx->channel = channel;
|
|
rx->mode = mode;
|
|
}
|
|
|
|
static inline void rx_frame(hdlc_rx_t *rx, uint8_t *data, int length)
|
|
{
|
|
uint16_t crc;
|
|
|
|
#ifdef DEBUG_HDLC
|
|
int i;
|
|
printf("frame:");
|
|
for (i = 0; i < length; i++) {
|
|
printf(" %02x", data[i]);
|
|
}
|
|
#endif
|
|
crc = crc16(data, length - 2);
|
|
if (data[length - 2] == (crc & 0xff) && data[length - 1] == (crc >> 8)) {
|
|
#ifdef DEBUG_HDLC
|
|
printf(" crc ok.\n");
|
|
#endif
|
|
ph_socket_tx_msg(&rx->uk0->ph_socket, rx->channel, PH_PRIM_DATA_IND, data, length - 2);
|
|
} else {
|
|
#ifdef DEBUG_HDLC
|
|
printf(" crc error!\n");
|
|
#endif
|
|
}
|
|
}
|
|
|
|
static inline void rx_bit(hdlc_rx_t *rx, uint8_t bit)
|
|
{
|
|
rx->frame <<= 1;
|
|
rx->frame |= bit;
|
|
|
|
switch (rx->state) {
|
|
case HDLC_STATE_IDLE:
|
|
if (rx->frame == 0x7e) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("received start flag.\n");
|
|
#endif
|
|
rx->state = HDLC_STATE_RX_FRAME;
|
|
rx->index = 0;
|
|
rx->bits = 0;
|
|
}
|
|
break;
|
|
case HDLC_STATE_RX_FRAME:
|
|
#ifdef DEBUG_HDLC
|
|
printf("received bit: %d\n", bit);
|
|
#endif
|
|
/* remove stuffed 0-bit after 5 1-bits */
|
|
if ((rx->frame & 0x7f) == 0x3e) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("omit stuffed 0-bit.\n");
|
|
#endif
|
|
break;
|
|
}
|
|
if ((rx->frame & 0x7f) == 0x7f) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("received frame abort!\n");
|
|
#endif
|
|
rx->state = HDLC_STATE_IDLE;
|
|
break;
|
|
}
|
|
if ((rx->frame & 0x7f) == 0x3f) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("received 6 1-bits.\n");
|
|
#endif
|
|
}
|
|
rx->buffer[rx->index] >>= 1;
|
|
rx->buffer[rx->index] |= (bit << 7);
|
|
if (++rx->bits == 8) {
|
|
rx->bits = 0;
|
|
rx->index++;
|
|
if (rx->index == sizeof(rx->buffer)) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("HDLC overflow!\n");
|
|
#endif
|
|
rx->state = HDLC_STATE_IDLE;
|
|
break;
|
|
}
|
|
}
|
|
if (rx->frame == 0x7e) {
|
|
#ifdef DEBUG_HDLC
|
|
printf("received end flag.\n");
|
|
#endif
|
|
if (rx->bits == 0) {
|
|
/* remove flag from buffer by substracting 1 from index */
|
|
if (rx->index > 3)
|
|
rx_frame(rx, rx->buffer, rx->index - 1);
|
|
} else {
|
|
#ifdef DEBUG_HDLC
|
|
printf("received frame does not end on byte boundary!\n");
|
|
#endif
|
|
}
|
|
rx->index = 0;
|
|
rx->bits = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break; /* should never happen */
|
|
}
|
|
}
|
|
|
|
/* data from interface going through HDLC to socket */
|
|
void uk0_receive(uk0_t *uk0, int ch, uint8_t *data, int length)
|
|
{
|
|
hdlc_rx_t *rx;
|
|
int i, j;
|
|
uint8_t bit;
|
|
|
|
if (ch < 1 || ch > 3) {
|
|
fprintf(stderr, "%s: illegal channel number %d!\n", __func__, ch);
|
|
return;
|
|
}
|
|
rx = &uk0->hdlc_rx[ch - 1];
|
|
|
|
/* disabled -> transparent */
|
|
switch (rx->mode) {
|
|
case HDLC_MODE_OFF:
|
|
break;
|
|
case HDLC_MODE_TRANS:
|
|
ph_socket_tx_msg(&rx->uk0->ph_socket, rx->channel, PH_PRIM_DATA_IND, data, length);
|
|
break;
|
|
case HDLC_MODE_HDLC:
|
|
for (i = 0; i < length; i++) {
|
|
for (j = 0; j < 8; j++) {
|
|
bit = (data[i] >> j) & 1;
|
|
rx_bit(rx, bit);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|