ax25_nrzi_g3ruh_scrambler.c

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <time.h>


uint32_t d_shift_register = 0;
uint32_t d_taps[32];
uint32_t d_tap_count;

void ax25_g3ruh_scrambler_init(uint32_t tap_mask) {
    uint8_t i;
    d_tap_count = 0;

    for (i=0; i<32; i++) {
        if (tap_mask & 0x01 == 1) {
            d_taps[d_tap_count] = i;
            d_tap_count++;
        }
        tap_mask = tap_mask >> 1;
    }
 
    d_shift_register = 0;
}
 
void ax25_g3ruh_scrambler(uint8_t* unscrambled, uint8_t* scrambled, uint16_t len) {
  uint8_t  unscrambled_bit;
  uint8_t  scrambled_bit;
  uint32_t tap_bit;
  uint16_t i, j, t;
 
  for (i=0; i<len; i++) {
        for (j=0; j<8; j++) {
            unscrambled_bit = unscrambled[i]>>j & 0x01;
            d_shift_register <<= 1;
 
            scrambled_bit = unscrambled_bit;
            for (t=0; t<d_tap_count; t++) {
                tap_bit = (d_shift_register >> d_taps[t]) & 0x01;
                scrambled_bit = scrambled_bit ^ tap_bit;
            }
 
            d_shift_register |= scrambled_bit;
 
            if (scrambled_bit)
                scrambled[i] |= (1 << j);
            else
                scrambled[i] &= ~(1 << j);
        }
    }
}
 
void ax25_g3ruh_descrambler(uint8_t* scrambled, uint8_t* unscrambled, uint16_t len) {
    uint8_t unscrambled_bit;
    uint8_t scrambled_bit;
    uint32_t tap_bit;
    uint16_t i, j, t;
 
    for (i=0; i<len; i++) {
        for (j=0; j<8; j++) {
            scrambled_bit = scrambled[i]>>j & 0x01;
            d_shift_register <<= 1;
 
            unscrambled_bit = scrambled_bit;
            for (t=0; t<d_tap_count; t++) {
                tap_bit = (d_shift_register >> d_taps[t]) & 0x01;
                unscrambled_bit = unscrambled_bit ^ tap_bit;
            }
 
            d_shift_register |= scrambled_bit;
 
            if (unscrambled_bit)
                unscrambled[i] |= (1 << j);
            else
                unscrambled[i] &= ~(1 << j);
        }
    }
}

void ax25_nrzi_encode(uint8_t* buf, uint8_t* output, uint16_t len) {
    uint8_t prev_nrzi_bit = 0;
    uint8_t nrz_bit;
    uint8_t nrzi_bit;
    uint16_t i, j;
 
    for (i=0; i<len; i++) {
        for (j=0; j<8; j++) {
            nrz_bit = buf[i]>>j & 0x01;
 
            if (nrz_bit == 0)
                nrzi_bit = prev_nrzi_bit ^ 1;
            else
                nrzi_bit = prev_nrzi_bit;
 
            if (nrzi_bit)
                output[i] |= (1 << j);
            else
                output[i] &= ~(1 << j);
 
            prev_nrzi_bit = nrzi_bit;
        }
    }
}

void ax25_nrzi_decode(uint8_t* buf, uint8_t* output, uint16_t len) {
    uint8_t prev_nrzi_bit = 0;
    uint8_t nrz_bit;
    uint8_t nrzi_bit;
    uint16_t i, j;
 
    for (i=0; i<len; i++) {
        for (j=0; j<8; j++) {
            nrzi_bit = buf[i]>>j & 0x01;
 
            if (nrzi_bit != prev_nrzi_bit)
                nrz_bit = 0;
            else
                nrz_bit = 1;
 
            if (nrz_bit)
                output[i] |= (1 << j);
            else
                output[i] &= ~(1 << j);
 
            prev_nrzi_bit = nrzi_bit;
        }
    }
}

int main() {
    uint8_t data[] = {0x7E, 0x7E, 0x7E, 0x7E, 0x7E, 0x00, 0x01, 0x02, 0x03};
    uint8_t i;

    printf("original data     : ");
    for (i=0; i<sizeof(data); i++) {
        printf("%02X ", data[i]);
    }
    printf("\n");

    ax25_nrzi_encode(data, data, sizeof(data));
    printf("nrzi encoded      : ");
    for (i=0; i<sizeof(data); i++) {
        printf("%02X ", data[i]);
    }
    printf("\n");

    ax25_g3ruh_scrambler_init(0x21000UL);
    ax25_g3ruh_scrambler(data, data, sizeof(data));
    printf("g3ruh scrambled   : ");
    for (i=0; i<sizeof(data); i++) {
        printf("%02X ", data[i]);
    }
    printf("\n\n");

    srand(time(NULL));
    // simulate out of sync descrambler, it may take 3 bytes to resync
    // that is why punch of 7E is sent before real data
    d_shift_register = rand() % 0xFFFFFFFF + 1;
    ax25_g3ruh_descrambler(data, data, sizeof(data));
    printf("g3ruh descrambled : ");
    for (i=0; i<sizeof(data); i++) {
        printf("%02X ", data[i]);
    }
    printf("\n");

    ax25_nrzi_decode(data, data, sizeof(data));
    printf("nrzi decoded      : ");
    for (i=0; i<sizeof(data); i++) {
        printf("%02X ", data[i]);
    }
    printf("\n");
    return 0;
}