From 591796fb99d4c9cc8c327de8f5d5b9f80328f3ed Mon Sep 17 00:00:00 2001 From: Morgan Redfield Date: Sun, 3 Jun 2018 21:25:22 -0700 Subject: [PATCH] split phy out into its own fileset (bell202) --- src/{packet.cpp => AX25.cpp} | 216 +++++++----------------- src/AX25.h | 285 ++++++++++++++++++++++++++++++++ src/KISS.cpp | 24 +-- src/KISS.h | 8 +- src/bell202.cpp | 132 +++++++++++++++ src/bell202.h | 83 ++++++++++ src/packet.h | 307 +---------------------------------- 7 files changed, 580 insertions(+), 475 deletions(-) rename src/{packet.cpp => AX25.cpp} (75%) create mode 100644 src/AX25.h create mode 100644 src/bell202.cpp create mode 100644 src/bell202.h diff --git a/src/packet.cpp b/src/AX25.cpp similarity index 75% rename from src/packet.cpp rename to src/AX25.cpp index 7f2a386..0a48a88 100644 --- a/src/packet.cpp +++ b/src/AX25.cpp @@ -1,36 +1,46 @@ #include #include "SimpleFIFO.h" -#include "packet.h" -#include "dds.h" +#include "AX25.h" +#include "bell202.h" #include -#define PHASE_BIT 8 -#define PHASE_INC 1 - -#define PHASE_MAX (SAMPLEPERBIT * PHASE_BIT) -#define PHASE_THRES (PHASE_MAX / 2) - -#define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01) -#define EDGE_FOUND(bitline) BIT_DIFFER((bitline), (bitline) >> 1) #define PPOOL_SIZE 2 -#define AFSK_SPACE 2200 -#define AFSK_MARK 1200 // Timers volatile unsigned long lastTx = 0; volatile unsigned long lastTxEnd = 0; volatile unsigned long lastRx = 0; -#define T_BIT ((unsigned int)(SAMPLERATE/BITRATE)) +Bell202 afsk; #ifdef PACKET_PREALLOCATE -SimpleFIFO preallocPool; -AFSK::Packet preallocPackets[PPOOL_SIZE]; +SimpleFIFO preallocPool; +AX25::Packet preallocPackets[PPOOL_SIZE]; #endif -void AFSK::Encoder::process() { + +bool AX25::enabled() { return afsk.enabled(); } +void AX25::start(DDS *d) { afsk.start(d); } + +// Determine what we want to do on this ADC tick. +void AX25::timer() { + static uint8_t tcnt = 0; + if(++tcnt == T_BIT && afsk.isSending()) { + PORTD |= _BV(6); + // Only run the encoder every 8th tick + // This is actually DDS RefClk / 1200 = 8, set as T_BIT + // A different refclk needs a different value + encoder.process(); + tcnt = 0; + PORTD &= ~_BV(6); + } else { + decoder.process((int8_t)(ADCH - ADC_OFFSET)); + } +} + +void AX25::Encoder::process() { // We're on the start of a byte position, so fetch one if(bitPosition == 0) { if(preamble) { // Still in preamble @@ -105,20 +115,15 @@ void AFSK::Encoder::process() { // NRZI and AFSK uses toggling 0s, "no change" on 1 // So, if not a 1, toggle to the opposite tone + bool currentTone = afsk.encoderGetTone(); if(!currentBit) currentTone = !currentTone; - if(currentTone == 0) { - PORTD |= _BV(7); - dds->setFrequency(AFSK_SPACE); - } else { - PORTD &= ~_BV(7); - dds->setFrequency(AFSK_MARK); - } + afsk.encoderSetTone(currentTone); } -bool AFSK::Encoder::start() { - if(!done || sending) { +bool AX25::Encoder::start() { + if(!done || afsk.isSending()) { return false; } @@ -137,28 +142,26 @@ bool AFSK::Encoder::start() { packet = 0x0; // No initial packet, find in the ISR currentBytePos = 0; maxTx = 3; - sending = true; nextByte = 0; - dds->setFrequency(0); - dds->on(); + + afsk.encoderStart(); return true; } -void AFSK::Encoder::stop() { +void AX25::Encoder::stop() { randomWait = 0; - sending = false; done = true; - dds->setFrequency(0); - dds->off(); + + afsk.encoderStop(); } -AFSK::Decoder::Decoder() { +AX25::Decoder::Decoder() { // Initialize the sampler delay line (phase shift) //for(unsigned char i = 0; i < SAMPLEPERBIT/2; i++) // delay_fifo.enqueue(0); } -bool AFSK::HDLCDecode::hdlcParse(bool bit, SimpleFIFO *fifo) { +bool AX25::HDLCDecode::hdlcParse(bool bit, SimpleFIFO *fifo) { bool ret = true; demod_bits <<= 1; @@ -205,70 +208,18 @@ bool AFSK::HDLCDecode::hdlcParse(bool bit, SimpleFIFO return ret; } -template -class FastRing { -private: - T ring[size]; - uint8_t position; -public: - FastRing(): position(0) {} - inline void write(T value) { - ring[(position++) & (size-1)] = value; - } - inline T read() const { - return ring[position & (size-1)]; - } - inline T readn(uint8_t n) const { - return ring[(position + (~n+1)) & (size-1)]; - } -}; -// Create a delay line that's half the length of the bit cycle (-90 degrees) -FastRing delayLine; - // Handle the A/D converter interrupt (hopefully quickly :) -void AFSK::Decoder::process(int8_t curr_sample) { - // Run the same through the phase multiplier and butterworth filter - iir_x[0] = iir_x[1]; - iir_x[1] = ((int8_t)delayLine.read() * curr_sample) >> 2; - iir_y[0] = iir_y[1]; - iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0]>>3) + (iir_y[0]>>5); - - // Place this ADC sample into the delay line - delayLine.write(curr_sample); - - // Shift the bit into place based on the output of the discriminator - sampled_bits <<= 1; - sampled_bits |= (iir_y[1] > 0) ? 1 : 0; - - // If we found a 0/1 transition, adjust phases to track - if(EDGE_FOUND(sampled_bits)) { - if(curr_phase < PHASE_THRES) - curr_phase += PHASE_INC; - else - curr_phase -= PHASE_INC; - } - - // Move ahead in phase - curr_phase += PHASE_BIT; - - // If we've gone over the phase maximum, we should now have some data - if(curr_phase >= PHASE_MAX) { - curr_phase %= PHASE_MAX; - found_bits <<= 1; +void AX25::Decoder::process(int8_t adc_val) { + bool parse_ready = afsk.decoderProcess(adc_val); - // If we have 3 bits or more set, it's a positive bit - register uint8_t bits = sampled_bits & 0x07; - if(bits == 0x07 || bits == 0x06 || bits == 0x05 || bits == 0x03) { - found_bits |= 1; - } - - hdlc.hdlcParse(!EDGE_FOUND(found_bits), &rx_fifo); // Process it + if (parse_ready) { + hdlc.hdlcParse(!EDGE_FOUND(afsk.found_bits), &rx_fifo); // Process it } } // This routine uses a pre-allocated Packet structure // to save on the memory requirements of the stream data -bool AFSK::Decoder::read() { +bool AX25::Decoder::read() { bool retVal = false; if(!currentPacket) { // We failed a prior memory allocation currentPacket = pBuf.makePacket(PACKET_MAX_LEN); @@ -350,42 +301,13 @@ bool AFSK::Decoder::read() { return retVal; // This is true if we parsed a packet in this flow } -#define AFSK_ADC_INPUT 2 -void AFSK::Decoder::start() { +void AX25::Decoder::start() { // Do this in start to allocate our first packet currentPacket = pBuf.makePacket(PACKET_MAX_LEN); -/* ASSR &= ~(_BV(EXCLK) | _BV(AS2)); - - // Do non-inverting PWM on pin OC2B (arduino pin 3) (p.159). - // OC2A (arduino pin 11) stays in normal port operation: - // COM2B1=1, COM2B0=0, COM2A1=0, COM2A0=0 - // Mode 1 - Phase correct PWM - TCCR2A = (TCCR2A | _BV(COM2B1)) & ~(_BV(COM2B0) | _BV(COM2A1) | _BV(COM2A0)) | - _BV(WGM21) | _BV(WGM20); - // No prescaler (p.162) - TCCR2B = (TCCR2B & ~(_BV(CS22) | _BV(CS21))) | _BV(CS20) | _BV(WGM22); - - OCR2A = pow(2,COMPARE_BITS)-1; - OCR2B = 0;*/ - - - // This lets us use decoding functions that run at the same reference - // clock as the DDS. - // We use ICR1 as TOP and prescale by 8 - // Note that this requires the DDS to be started as well - TCCR1A = 0; - TCCR1B = _BV(CS11) | _BV(WGM13) | _BV(WGM12); - ICR1 = ((F_CPU / 8) / 9600) - 1; //TODO: get the actual refclk from dds - // NOTE: should divider be 1 or 8? - ADMUX = _BV(REFS0) | _BV(ADLAR) | AFSK_ADC_INPUT; // Channel AFSK_ADC_INPUT, shift result left (ADCH used) - DDRC &= ~_BV(AFSK_ADC_INPUT); - PORTC &= ~_BV(AFSK_ADC_INPUT); - DIDR0 |= _BV(AFSK_ADC_INPUT); // disable input buffer for ADC pin - ADCSRB = _BV(ADTS2) | _BV(ADTS1) | _BV(ADTS0); - ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIE) | _BV(ADPS2); // | _BV(ADPS0); + afsk.decoderStart(); } -AFSK::PacketBuffer::PacketBuffer() { +AX25::PacketBuffer::PacketBuffer() { nextPacketIn = 0; nextPacketOut = 0; inBuffer = 0; @@ -400,7 +322,7 @@ AFSK::PacketBuffer::PacketBuffer() { #endif } -unsigned char AFSK::PacketBuffer::readyCount() volatile { +unsigned char AX25::PacketBuffer::readyCount() volatile { unsigned char i; unsigned int cnt = 0; ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { @@ -413,9 +335,9 @@ unsigned char AFSK::PacketBuffer::readyCount() volatile { } // Return NULL on empty packet buffers -AFSK::Packet *AFSK::PacketBuffer::getPacket() volatile { +AX25::Packet *AX25::PacketBuffer::getPacket() volatile { unsigned char i = 0; - AFSK::Packet *p = NULL; + AX25::Packet *p = NULL; ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { if(inBuffer == 0) { @@ -438,7 +360,7 @@ AFSK::Packet *AFSK::PacketBuffer::getPacket() volatile { } //void Packet::init(uint8_t *buf, unsigned int dlen, bool freeData) { -void AFSK::Packet::init(unsigned short dlen) { +void AX25::Packet::init(unsigned short dlen) { //data = (unsigned char *)buf; ready = 0; #ifdef PACKET_PREALLOCATE @@ -457,8 +379,8 @@ void AFSK::Packet::init(unsigned short dlen) { } // Allocate a new packet with a data buffer as set -AFSK::Packet *AFSK::PacketBuffer::makePacket(unsigned short dlen) { - AFSK::Packet *p; +AX25::Packet *AX25::PacketBuffer::makePacket(unsigned short dlen) { + AX25::Packet *p; ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { //Packet *p = findPooledPacket(); #ifdef PACKET_PREALLOCATE @@ -475,7 +397,7 @@ AFSK::Packet *AFSK::PacketBuffer::makePacket(unsigned short dlen) { } // Free a packet struct, mainly convenience -void AFSK::PacketBuffer::freePacket(Packet *p) { +void AX25::PacketBuffer::freePacket(Packet *p) { if(!p) return; ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { @@ -495,7 +417,7 @@ void AFSK::PacketBuffer::freePacket(Packet *p) { } // Put a packet onto the buffer array -bool AFSK::PacketBuffer::putPacket(Packet *p) volatile { +bool AX25::PacketBuffer::putPacket(Packet *p) volatile { ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { if(inBuffer >= PACKET_BUFFER_SIZE) { return false; @@ -508,11 +430,11 @@ bool AFSK::PacketBuffer::putPacket(Packet *p) volatile { } // Print a single byte to the data array -size_t AFSK::Packet::write(uint8_t c) { +size_t AX25::Packet::write(uint8_t c) { return (appendFCS(c)?1:0); } -size_t AFSK::Packet::write(const uint8_t *ptr, size_t len) { +size_t AX25::Packet::write(const uint8_t *ptr, size_t len) { size_t i; for(i = 0; i < len; ++i) if(!appendFCS(ptr[i])) @@ -522,7 +444,7 @@ size_t AFSK::Packet::write(const uint8_t *ptr, size_t len) { // Add a callsign, flagged as source, destination, or digi // Also tell the routine the SSID to use and if this is the final callsign -size_t AFSK::Packet::appendCallsign(const char *callsign, uint8_t ssid, bool final) { +size_t AX25::Packet::appendCallsign(const char *callsign, uint8_t ssid, bool final) { uint8_t i; for(i = 0; i < strlen(callsign) && i < 6; i++) { appendFCS(callsign[i]<<1); @@ -544,7 +466,7 @@ size_t AFSK::Packet::appendCallsign(const char *callsign, uint8_t ssid, bool fin #ifdef PACKET_PARSER // Process the AX25 frame and turn it into a bunch of useful strings -bool AFSK::Packet::parsePacket() { +bool AX25::Packet::parsePacket() { uint8_t *d = dataPtr; int i; @@ -619,7 +541,7 @@ bool AFSK::Packet::parsePacket() { } #endif -void AFSK::Packet::printPacket(Stream *s) { +void AX25::Packet::printPacket(Stream *s) { uint8_t i; #ifdef PACKET_PARSER if(!parsePacket()) { @@ -722,25 +644,3 @@ void AFSK::Packet::printPacket(Stream *s) { #endif } -// Determine what we want to do on this ADC tick. -void AFSK::timer() { - static uint8_t tcnt = 0; - if(++tcnt == T_BIT && encoder.isSending()) { - PORTD |= _BV(6); - // Only run the encoder every 8th tick - // This is actually DDS RefClk / 1200 = 8, set as T_BIT - // A different refclk needs a different value - encoder.process(); - tcnt = 0; - PORTD &= ~_BV(6); - } else { - //decoder.process(((int8_t)(ADCH - 128))); - decoder.process((int8_t)(ADCH - 83)); - } -} - -void AFSK::start(DDS *dds) { - afskEnabled = true; - encoder.setDDS(dds); - decoder.start(); -} diff --git a/src/AX25.h b/src/AX25.h new file mode 100644 index 0000000..2ca0845 --- /dev/null +++ b/src/AX25.h @@ -0,0 +1,285 @@ +#ifndef _AX25_H_ +#define _AX25_H_ + +#include +#include +#include +#include + +#define RX_FIFO_LEN 16 + +#define AFSK_RX_FIFO_LEN 16 +#define PACKET_BUFFER_SIZE 2 +#define PACKET_STATIC 0 + +// Enable the packet parser which will tokenize the AX25 frame into easy strings +#define PACKET_PARSER + +// If this is set, all the packet buffers will be pre-allocated at compile time +// This will use more RAM, but can make it easier to do memory planning. +// TODO: Make this actually work right and not crash. +#define PACKET_PREALLOCATE + +// This is with all the digis, two addresses, and full payload +// Dst(7) + Src(7) + Digis(56) + Ctl(1) + PID(1) + Data(0-256) + FCS(2) +#define PACKET_MAX_LEN 330 +// Minimum is Dst + Src + Ctl + FCS +#define AX25_PACKET_HEADER_MINLEN 17 + +// HDLC framing bits +#define HDLC_FRAME 0x7E +#define HDLC_RESET 0x7F +#define HDLC_PREAMBLE 0x00 +#define HDLC_ESCAPE 0x1B +#define HDLC_TAIL 0x1C + +class AX25 { +public: + bool enabled(); + void start(DDS *d); + void timer(); + + class Packet:public Print { + public: + Packet():Print() {}; + virtual size_t write(uint8_t); + // Stock virtual method does what we want here. + //virtual size_t write(const char *); + virtual size_t write(const uint8_t *, size_t); + using Print::write; + unsigned char ready : 1; + unsigned char type : 2; + unsigned char freeData : 1; + unsigned short len; + unsigned short maxLen; + //void init(uint8_t *buf, unsigned int dlen, bool freeData); + void init(unsigned short dlen); + inline void free() { + if(freeData) + ::free(dataPtr); + } + inline const unsigned char getByte(void) { + return *readPos++; + } + inline const unsigned char getByte(uint16_t p) { + return *(dataPtr+p); + } + inline void start() { + fcs = 0xffff; + // No longer put an explicit frame start here + //*dataPos++ = HDLC_ESCAPE; + //*dataPos++ = HDLC_FRAME; + //len = 2; + len = 0; + } + + inline bool append(char c) { + if(len < maxLen) { + ++len; + *dataPos++ = c; + return true; + } + return false; + } + + #define UPDATE_FCS(d) e=fcs^(d); f=e^(e<<4); fcs=(fcs>>8)^(f<<8)^(f<<3)^(f>>4) + //#define UPDATE_FCS(d) s=(d)^(fcs>>8); t=s^(s>>4); fcs=(fcs<<8)^t^(t<<5)^(t<<12) + inline bool appendFCS(unsigned char c) { + register unsigned char e, f; + if(len < maxLen - 4) { // Leave room for FCS/HDLC + append(c); + UPDATE_FCS(c); + return true; + } + return false; + } + + size_t appendCallsign(const char *callsign, uint8_t ssid, bool final = false); + + inline void finish() { + append(~(fcs & 0xff)); + append(~((fcs>>8) & 0xff)); + // No longer append the frame boundaries themselves + //append(HDLC_ESCAPE); + //append(HDLC_FRAME); + ready = 1; + } + + inline void clear() { + fcs = 0xffff; + len = 0; + readPos = dataPtr; + dataPos = dataPtr; + } + + inline bool crcOK() { + return (fcs == 0xF0B8); + } +#ifdef PACKET_PARSER + bool parsePacket(); +#endif + void printPacket(Stream *s); + private: +#ifdef PACKET_PREALLOCATE + uint8_t dataPtr[PACKET_MAX_LEN]; // 256 byte I frame + headers max of 78 +#else + uint8_t *dataPtr; +#endif +#ifdef PACKET_PARSER + char srcCallsign[7]; + uint8_t srcSSID; + char dstCallsign[7]; + uint8_t dstSSID; + char digipeater[8][7]; + uint8_t digipeaterSSID[8]; + uint8_t *iFrameData; + uint8_t length; + uint8_t control; + uint8_t pid; +#endif + uint8_t *dataPos, *readPos; + unsigned short fcs; + }; + + + class PacketBuffer { + public: + // Initialize the buffers + PacketBuffer(); + // How many packets are in the buffer? + unsigned char count() volatile { return inBuffer; }; + // And how many of those are ready? + unsigned char readyCount() volatile; + // Retrieve the next packet + Packet *getPacket() volatile; + // Create a packet structure as needed + // This does not place it in the queue + static Packet *makePacket(unsigned short); + // Conveniently free packet memory + static void freePacket(Packet *); + // Place a packet into the buffer + bool putPacket(Packet *) volatile; + private: + volatile unsigned char inBuffer; + Packet * volatile packets[PACKET_BUFFER_SIZE]; + volatile unsigned char nextPacketIn; + volatile unsigned char nextPacketOut; + }; + + class Encoder { + public: + Encoder() { + randomWait = 1000; // At the very begin, wait at least one second + done = true; + packet = 0x0; + currentBytePos = 0; + nextByte = 0; + } + void setDDS(DDS *d) { dds = d; } + //int16 getReferenceClock() { return dds.getReferenceClock(); } + volatile inline bool isDone() volatile { + return done; + } + volatile inline bool hasPackets() volatile { + return (pBuf.count() > 0); + } + inline bool putPacket(Packet *packet) { + return pBuf.putPacket(packet); + } + inline void setRandomWait() { + randomWait = 250 + (rand() % 1000) + millis(); + } + bool start(); + void stop(); + void process(); + private: + byte currentByte; + byte currentBit : 1; + byte lastZero : 3; + byte bitPosition : 3; + byte preamble : 6; + //byte bitClock; + bool hdlc; + byte nextByte; + byte maxTx; + Packet *packet; + PacketBuffer pBuf; + unsigned int currentBytePos; + volatile unsigned long randomWait; + volatile bool done; + DDS *dds; + }; + + class HDLCDecode { + public: + bool hdlcParse(bool, SimpleFIFO *fifo); + volatile bool rxstart; + private: + uint8_t demod_bits; + uint8_t bit_idx; + uint8_t currchar; + }; + + class Decoder { + public: + Decoder(); + void start(); + bool read(); + void process(int8_t); + inline bool dataAvailable() { + return (rx_fifo.count() > 0); + } + inline uint8_t getByte() { + return rx_fifo.dequeue(); + } + inline uint8_t packetCount() volatile { + return pBuf.count(); + } + inline Packet *getPacket() { + return pBuf.getPacket(); + } + inline bool isReceiving() volatile { + return hdlc.rxstart; + } + private: + Packet *currentPacket; + SimpleFIFO rx_fifo; // This should be drained fairly often + PacketBuffer pBuf; + HDLCDecode hdlc; + }; + +public: + inline bool read() { + return decoder.read(); + } + volatile inline bool txReady() volatile { + if(encoder.isDone() && encoder.hasPackets()) + return true; + return false; + } + volatile inline bool isDone() volatile { return encoder.isDone(); } + inline bool txStart() { + if(decoder.isReceiving()) { + encoder.setRandomWait(); + return false; + } + return encoder.start(); + } + inline bool putTXPacket(Packet *packet) { + bool ret = encoder.putPacket(packet); + if(!ret) // No room? + PacketBuffer::freePacket(packet); + return ret; + } + inline Packet *getRXPacket() { + return decoder.getPacket(); + } + inline uint8_t rxPacketCount() volatile { + return decoder.packetCount(); + } + //unsigned long lastTx; + //unsigned long lastRx; + Encoder encoder; + Decoder decoder; +}; +#endif /* _AX25_H_ */ \ No newline at end of file diff --git a/src/KISS.cpp b/src/KISS.cpp index e4e0597..d1951fe 100644 --- a/src/KISS.cpp +++ b/src/KISS.cpp @@ -1,8 +1,8 @@ #include -#include "packet.h" +#include "AX25.h" #include "KISS.h" -//AFSK::Packet kissPacket; +//AX25::Packet kissPacket; bool inFrame = false; uint8_t kissBuffer[PACKET_MAX_LEN]; uint16_t kissLen = 0; @@ -11,14 +11,14 @@ uint16_t kissLen = 0; // KISS equipment, and look if we have anything to relay along void KISS::loop() { static bool currentlySending = false; - if(afsk->decoder.read() || afsk->rxPacketCount()) { + if(ax25->decoder.read() || ax25->rxPacketCount()) { // A true return means something was put onto the packet FIFO // If we actually have data packets in the buffer, process them all now - while(afsk->rxPacketCount()) { - AFSK::Packet *packet = afsk->getRXPacket(); + while(ax25->rxPacketCount()) { + AX25::Packet *packet = ax25->getRXPacket(); if(packet) { writePacket(packet); - AFSK::PacketBuffer::freePacket(packet); + AX25::PacketBuffer::freePacket(packet); } } } @@ -28,13 +28,13 @@ void KISS::loop() { if(c == KISS_FEND) { if(inFrame && kissLen > 0) { int i; - AFSK::Packet *packet = AFSK::PacketBuffer::makePacket(PACKET_MAX_LEN); + AX25::Packet *packet = AX25::PacketBuffer::makePacket(PACKET_MAX_LEN); packet->start(); for(i = 0; i < kissLen; i++) { packet->appendFCS(kissBuffer[i]); } packet->finish(); - afsk->encoder.putPacket(packet); + ax25->encoder.putPacket(packet); } kissLen = 0; inFrame = false; @@ -57,21 +57,21 @@ void KISS::loop() { inFrame = true; } } - if(afsk->txReady()) { + if(ax25->txReady()) { radio->setModeTransmit(); currentlySending = true; - if(!afsk->txStart()) { // Unable to start for some reason + if(!ax25->txStart()) { // Unable to start for some reason radio->setModeReceive(); currentlySending = false; } } - if(currentlySending && afsk->encoder.isDone()) { + if(currentlySending && ax25->encoder.isDone()) { radio->setModeReceive(); currentlySending = false; } } -void KISS::writePacket(AFSK::Packet *p) { +void KISS::writePacket(AX25::Packet *p) { int i; io->write(KISS_FEND); io->write((uint8_t)0); // Host to TNC port identifier diff --git a/src/KISS.h b/src/KISS.h index a5f3002..6df4021 100644 --- a/src/KISS.h +++ b/src/KISS.h @@ -2,7 +2,7 @@ #define _KISS_H_ #include -#include "packet.h" +#include "AX25.h" #define KISS_FEND 0xC0 #define KISS_FESC 0xDB @@ -11,15 +11,15 @@ class KISS { public: - KISS(Stream *_io, HamShield *h, DDS *d, AFSK *a) : io(_io), radio(h), dds(d), afsk(a) {} + KISS(Stream *_io, HamShield *h, DDS *d, AX25 *a) : io(_io), radio(h), dds(d), ax25(a) {} bool read(); - void writePacket(AFSK::Packet *); + void writePacket(AX25::Packet *); void loop(); private: Stream *io; HamShield *radio; DDS *dds; - AFSK *afsk; + AX25 *ax25; }; #endif /* _KISS_H_ */ diff --git a/src/bell202.cpp b/src/bell202.cpp new file mode 100644 index 0000000..19ecf12 --- /dev/null +++ b/src/bell202.cpp @@ -0,0 +1,132 @@ +#include +#include "dds.h" +#include "bell202.h" + + +#define T_BIT ((unsigned int)(SAMPLERATE/BITRATE)) + +bool Bell202::encoderStart() { + sending = true; + dds->setFrequency(0); + dds->on(); + return true; +} +void Bell202::encoderStop() { + sending = false; + dds->setFrequency(0); + dds->off(); +} + +void Bell202::encoderSetTone(bool tone) { + currentTone = tone; + if(tone == 0) { + PORTD |= _BV(7); + dds->setFrequency(AFSK_SPACE); + } else { + PORTD &= ~_BV(7); + dds->setFrequency(AFSK_MARK); + } +} + +template +class FastRing { +private: + T ring[size]; + uint8_t position; +public: + FastRing(): position(0) {} + inline void write(T value) { + ring[(position++) & (size-1)] = value; + } + inline T read() const { + return ring[position & (size-1)]; + } + inline T readn(uint8_t n) const { + return ring[(position + (~n+1)) & (size-1)]; + } +}; +// Create a delay line that's half the length of the bit cycle (-90 degrees) +FastRing delayLine; + +// Handle the A/D converter interrupt (hopefully quickly :) +bool Bell202::decoderProcess(int8_t curr_sample) { + // Run the same through the phase multiplier and butterworth filter + iir_x[0] = iir_x[1]; + iir_x[1] = ((int8_t)delayLine.read() * curr_sample) >> 2; + iir_y[0] = iir_y[1]; + iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0]>>3) + (iir_y[0]>>5); + + // Place this ADC sample into the delay line + delayLine.write(curr_sample); + + // Shift the bit into place based on the output of the discriminator + sampled_bits <<= 1; + sampled_bits |= (iir_y[1] > 0) ? 1 : 0; + + // If we found a 0/1 transition, adjust phases to track + if(EDGE_FOUND(sampled_bits)) { + if(curr_phase < PHASE_THRES) + curr_phase += PHASE_INC; + else + curr_phase -= PHASE_INC; + } + + // Move ahead in phase + curr_phase += PHASE_BIT; + + // If we've gone over the phase maximum, we should now have some data + if(curr_phase >= PHASE_MAX) { + curr_phase %= PHASE_MAX; + found_bits <<= 1; + + // If we have 3 bits or more set, it's a positive bit + register uint8_t bits = sampled_bits & 0x07; + if(bits == 0x07 || bits == 0x06 || bits == 0x05 || bits == 0x03) { + found_bits |= 1; + } + + // pass back to encoder + return true; + } + return false; +} + + +#define AFSK_ADC_INPUT 2 +void Bell202::decoderStart() { + /* ASSR &= ~(_BV(EXCLK) | _BV(AS2)); + + // Do non-inverting PWM on pin OC2B (arduino pin 3) (p.159). + // OC2A (arduino pin 11) stays in normal port operation: + // COM2B1=1, COM2B0=0, COM2A1=0, COM2A0=0 + // Mode 1 - Phase correct PWM + TCCR2A = (TCCR2A | _BV(COM2B1)) & ~(_BV(COM2B0) | _BV(COM2A1) | _BV(COM2A0)) | + _BV(WGM21) | _BV(WGM20); + // No prescaler (p.162) + TCCR2B = (TCCR2B & ~(_BV(CS22) | _BV(CS21))) | _BV(CS20) | _BV(WGM22); + + OCR2A = pow(2,COMPARE_BITS)-1; + OCR2B = 0;*/ + + + // This lets us use decoding functions that run at the same reference + // clock as the DDS. + // We use ICR1 as TOP and prescale by 8 + // Note that this requires the DDS to be started as well + TCCR1A = 0; + TCCR1B = _BV(CS11) | _BV(WGM13) | _BV(WGM12); + ICR1 = ((F_CPU / 8) / 9600) - 1; //TODO: get the actual refclk from dds + // NOTE: should divider be 1 or 8? + ADMUX = _BV(REFS0) | _BV(ADLAR) | AFSK_ADC_INPUT; // Channel AFSK_ADC_INPUT, shift result left (ADCH used) + DDRC &= ~_BV(AFSK_ADC_INPUT); + PORTC &= ~_BV(AFSK_ADC_INPUT); + DIDR0 |= _BV(AFSK_ADC_INPUT); // disable input buffer for ADC pin + ADCSRB = _BV(ADTS2) | _BV(ADTS1) | _BV(ADTS0); + ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIE) | _BV(ADPS2); // | _BV(ADPS0); +} + +void Bell202::start(DDS *dds) { + afskEnabled = true; + encoderSetDDS(dds); + decoderStart(); +} diff --git a/src/bell202.h b/src/bell202.h new file mode 100644 index 0000000..5057ed2 --- /dev/null +++ b/src/bell202.h @@ -0,0 +1,83 @@ +#ifndef _BELL202_H_ +#define _BELL202_H_ + +/* + * bell202.h + * The Bell202 class implements an AFSK transceiver at the bit level. + * All encoding and framing should be done at a higher level (e.g. HDLC and AX.25) + * + * See http://destevez.net/2016/06/kiss-hdlc-ax-25-and-friends/ + */ + +#include +#include +#include + +#define SAMPLERATE 9600 +#define BITRATE 1200 + +#define SAMPLEPERBIT (SAMPLERATE / BITRATE) + +#define RX_FIFO_LEN 16 + +#define AFSK_RX_FIFO_LEN 16 + +#define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01) + +#define EDGE_FOUND(bitline) BIT_DIFFER((bitline), (bitline) >> 1) + +#define T_BIT ((unsigned int)(SAMPLERATE/BITRATE)) + +#define PHASE_BIT 8 +#define PHASE_INC 1 + +#define PHASE_MAX (SAMPLEPERBIT * PHASE_BIT) +#define PHASE_THRES (PHASE_MAX / 2) + +#define AFSK_SPACE 2200 +#define AFSK_MARK 1200 + +#define ADC_OFFSET 83 + +class Bell202 { +private: + volatile bool afskEnabled; +public: + Bell202() { + sending = false; + } + volatile inline bool enabled() { return afskEnabled; } + volatile inline bool isSending() volatile { + return sending; + } + + void encoderSetDDS(DDS *d) { dds = d; } + //int16 getReferenceClock() { return dds.getReferenceClock(); } + bool encoderStart(); + void encoderStop(); + void encoderSetTone(bool tone); + bool encoderGetTone() { return currentTone; } + + void decoderStart(); + bool decoderRead(); + bool decoderProcess(int8_t); + + void start(DDS *); + + uint8_t found_bits; + +private: + volatile bool sending; + byte currentTone : 1; + //byte bitClock; + DDS *dds; + + int16_t iir_x[2]; + int16_t iir_y[2]; + int8_t curr_phase; + uint8_t sampled_bits; + +}; + + +#endif /* _BELL202_H_ */ diff --git a/src/packet.h b/src/packet.h index c050b3e..7ed6664 100644 --- a/src/packet.h +++ b/src/packet.h @@ -1,304 +1,9 @@ -#ifndef _AFSK_H_ -#define _AFSK_H_ +#ifndef _PACKET_H_ +#define _PACKET_H_ -#include -#include -#include -#include +#include "AX25.h" +#include "bell202.h" -#define SAMPLERATE 9600 -#define BITRATE 1200 +#define AFSK AX25 // -#define SAMPLEPERBIT (SAMPLERATE / BITRATE) - -#define RX_FIFO_LEN 16 - -#define AFSK_RX_FIFO_LEN 16 -#define PACKET_BUFFER_SIZE 2 -#define PACKET_STATIC 0 - -// Enable the packet parser which will tokenize the AX25 frame into easy strings -#define PACKET_PARSER - -// If this is set, all the packet buffers will be pre-allocated at compile time -// This will use more RAM, but can make it easier to do memory planning. -// TODO: Make this actually work right and not crash. -#define PACKET_PREALLOCATE - -// This is with all the digis, two addresses, and full payload -// Dst(7) + Src(7) + Digis(56) + Ctl(1) + PID(1) + Data(0-256) + FCS(2) -#define PACKET_MAX_LEN 330 -// Minimum is Dst + Src + Ctl + FCS -#define AX25_PACKET_HEADER_MINLEN 17 - -// HDLC framing bits -#define HDLC_FRAME 0x7E -#define HDLC_RESET 0x7F -#define HDLC_PREAMBLE 0x00 -#define HDLC_ESCAPE 0x1B -#define HDLC_TAIL 0x1C - -class AFSK { -private: - volatile bool afskEnabled; -public: - bool enabled() { return afskEnabled; }; - - class Packet:public Print { - public: - Packet():Print() {}; - virtual size_t write(uint8_t); - // Stock virtual method does what we want here. - //virtual size_t write(const char *); - virtual size_t write(const uint8_t *, size_t); - using Print::write; - unsigned char ready : 1; - unsigned char type : 2; - unsigned char freeData : 1; - unsigned short len; - unsigned short maxLen; - //void init(uint8_t *buf, unsigned int dlen, bool freeData); - void init(unsigned short dlen); - inline void free() { - if(freeData) - ::free(dataPtr); - } - inline const unsigned char getByte(void) { - return *readPos++; - } - inline const unsigned char getByte(uint16_t p) { - return *(dataPtr+p); - } - inline void start() { - fcs = 0xffff; - // No longer put an explicit frame start here - //*dataPos++ = HDLC_ESCAPE; - //*dataPos++ = HDLC_FRAME; - //len = 2; - len = 0; - } - - inline bool append(char c) { - if(len < maxLen) { - ++len; - *dataPos++ = c; - return true; - } - return false; - } - - #define UPDATE_FCS(d) e=fcs^(d); f=e^(e<<4); fcs=(fcs>>8)^(f<<8)^(f<<3)^(f>>4) - //#define UPDATE_FCS(d) s=(d)^(fcs>>8); t=s^(s>>4); fcs=(fcs<<8)^t^(t<<5)^(t<<12) - inline bool appendFCS(unsigned char c) { - register unsigned char e, f; - if(len < maxLen - 4) { // Leave room for FCS/HDLC - append(c); - UPDATE_FCS(c); - return true; - } - return false; - } - - size_t appendCallsign(const char *callsign, uint8_t ssid, bool final = false); - - inline void finish() { - append(~(fcs & 0xff)); - append(~((fcs>>8) & 0xff)); - // No longer append the frame boundaries themselves - //append(HDLC_ESCAPE); - //append(HDLC_FRAME); - ready = 1; - } - - inline void clear() { - fcs = 0xffff; - len = 0; - readPos = dataPtr; - dataPos = dataPtr; - } - - inline bool crcOK() { - return (fcs == 0xF0B8); - } -#ifdef PACKET_PARSER - bool parsePacket(); -#endif - void printPacket(Stream *s); - private: -#ifdef PACKET_PREALLOCATE - uint8_t dataPtr[PACKET_MAX_LEN]; // 256 byte I frame + headers max of 78 -#else - uint8_t *dataPtr; -#endif -#ifdef PACKET_PARSER - char srcCallsign[7]; - uint8_t srcSSID; - char dstCallsign[7]; - uint8_t dstSSID; - char digipeater[8][7]; - uint8_t digipeaterSSID[8]; - uint8_t *iFrameData; - uint8_t length; - uint8_t control; - uint8_t pid; -#endif - uint8_t *dataPos, *readPos; - unsigned short fcs; - }; - - - class PacketBuffer { - public: - // Initialize the buffers - PacketBuffer(); - // How many packets are in the buffer? - unsigned char count() volatile { return inBuffer; }; - // And how many of those are ready? - unsigned char readyCount() volatile; - // Retrieve the next packet - Packet *getPacket() volatile; - // Create a packet structure as needed - // This does not place it in the queue - static Packet *makePacket(unsigned short); - // Conveniently free packet memory - static void freePacket(Packet *); - // Place a packet into the buffer - bool putPacket(Packet *) volatile; - private: - volatile unsigned char inBuffer; - Packet * volatile packets[PACKET_BUFFER_SIZE]; - volatile unsigned char nextPacketIn; - volatile unsigned char nextPacketOut; - }; - - class Encoder { - public: - Encoder() { - randomWait = 1000; // At the very begin, wait at least one second - sending = false; - done = true; - packet = 0x0; - currentBytePos = 0; - nextByte = 0; - } - void setDDS(DDS *d) { dds = d; } - //int16 getReferenceClock() { return dds.getReferenceClock(); } - volatile inline bool isSending() volatile { - return sending; - } - volatile inline bool isDone() volatile { - return done; - } - volatile inline bool hasPackets() volatile { - return (pBuf.count() > 0); - } - inline bool putPacket(Packet *packet) { - return pBuf.putPacket(packet); - } - inline void setRandomWait() { - randomWait = 250 + (rand() % 1000) + millis(); - } - bool start(); - void stop(); - void process(); - private: - volatile bool sending; - byte currentByte; - byte currentBit : 1; - byte currentTone : 1; - byte lastZero : 3; - byte bitPosition : 3; - byte preamble : 6; - //byte bitClock; - bool hdlc; - byte nextByte; - byte maxTx; - Packet *packet; - PacketBuffer pBuf; - unsigned int currentBytePos; - volatile unsigned long randomWait; - volatile bool done; - DDS *dds; - }; - - class HDLCDecode { - public: - bool hdlcParse(bool, SimpleFIFO *fifo); - volatile bool rxstart; - private: - uint8_t demod_bits; - uint8_t bit_idx; - uint8_t currchar; - }; - - class Decoder { - public: - Decoder(); - void start(); - bool read(); - void process(int8_t); - inline bool dataAvailable() { - return (rx_fifo.count() > 0); - } - inline uint8_t getByte() { - return rx_fifo.dequeue(); - } - inline uint8_t packetCount() volatile { - return pBuf.count(); - } - inline Packet *getPacket() { - return pBuf.getPacket(); - } - inline bool isReceiving() volatile { - return hdlc.rxstart; - } - private: - Packet *currentPacket; - //SimpleFIFO delay_fifo; - SimpleFIFO rx_fifo; // This should be drained fairly often - int16_t iir_x[2]; - int16_t iir_y[2]; - uint8_t sampled_bits; - int8_t curr_phase; - uint8_t found_bits; - PacketBuffer pBuf; - HDLCDecode hdlc; - }; - -public: - inline bool read() { - return decoder.read(); - } - volatile inline bool txReady() volatile { - if(encoder.isDone() && encoder.hasPackets()) - return true; - return false; - } - volatile inline bool isDone() volatile { return encoder.isDone(); } - inline bool txStart() { - if(decoder.isReceiving()) { - encoder.setRandomWait(); - return false; - } - return encoder.start(); - } - inline bool putTXPacket(Packet *packet) { - bool ret = encoder.putPacket(packet); - if(!ret) // No room? - PacketBuffer::freePacket(packet); - return ret; - } - inline Packet *getRXPacket() { - return decoder.getPacket(); - } - inline uint8_t rxPacketCount() volatile { - return decoder.packetCount(); - } - //unsigned long lastTx; - //unsigned long lastRx; - void start(DDS *); - void timer(); - Encoder encoder; - Decoder decoder; -}; -#endif /* _AFSK_H_ */ +#endif /* _PACKET_H_ */ \ No newline at end of file