From 10c2a26ed2797e34a290c628d209b7cfa8ab41d8 Mon Sep 17 00:00:00 2001 From: nick6x Date: Fri, 26 Aug 2016 11:12:16 -0700 Subject: [PATCH] first commit --- README.md | Bin 0 -> 38 bytes library.properties | 10 + src/KISS.cpp | 88 ++++++ src/KISS.h | 36 +++ src/SimpleFIFO.h | 89 ++++++ src/packet.cpp | 738 +++++++++++++++++++++++++++++++++++++++++++++ src/packet.h | 302 +++++++++++++++++++ 7 files changed, 1263 insertions(+) create mode 100644 README.md create mode 100644 library.properties create mode 100644 src/KISS.cpp create mode 100644 src/KISS.h create mode 100644 src/SimpleFIFO.h create mode 100644 src/packet.cpp create mode 100644 src/packet.h diff --git a/README.md b/README.md new file mode 100644 index 0000000000000000000000000000000000000000..c893eb19cd41cdf31e3c581433503b7480cc0d41 GIT binary patch literal 38 scmezWPnki1!Gj@@A(tVTA%h{4A(bJAA%!8H!JEMoC=$%T%fQ6|0JVMxU;qFB literal 0 HcmV?d00001 diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..dc5636e --- /dev/null +++ b/library.properties @@ -0,0 +1,10 @@ +name=HamShield_KISS +version=1.0.2 +author=Morgan Redfield , Casey Halverson +maintainer=Morgan Redfield +sentence=A library for use with HamShield by Enhanced Radio Devices. +paragraph= +category=Device Control +url=http://www.hamshield.com +architectures=* +includes=packet.h, SimpleFIFO.h, KISS.h \ No newline at end of file diff --git a/src/KISS.cpp b/src/KISS.cpp new file mode 100644 index 0000000..3f3e635 --- /dev/null +++ b/src/KISS.cpp @@ -0,0 +1,88 @@ +#include +#include "packet.h" +#include "KISS.h" + +//AFSK::Packet kissPacket; +bool inFrame = false; +uint8_t kissBuffer[PACKET_MAX_LEN]; +uint16_t kissLen = 0; + +// Inside the KISS loop, we basically wait for data to come in from the +// KISS equipment, and look if we have anything to relay along +void KISS::loop() { + static bool currentlySending = false; + if(afsk.decoder.read() || afsk.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(); + if(packet) { + writePacket(packet); + AFSK::PacketBuffer::freePacket(packet); + } + } + } + // Check if we have incoming data to turn into a packet + while(io->available()) { + uint8_t c = (uint8_t)io->read(); + if(c == KISS_FEND) { + if(inFrame && kissLen > 0) { + int i; + AFSK::Packet *packet = AFSK::PacketBuffer::makePacket(PACKET_MAX_LEN); + packet->start(); + for(i = 0; i < kissLen; i++) { + packet->appendFCS(kissBuffer[i]); + } + packet->finish(); + afsk.encoder.putPacket(packet); + } + kissLen = 0; + inFrame = false; + } + // We're inside the boundaries of a FEND + if(inFrame) { + // Unescape the incoming data + if(c == KISS_FESC) { + c = io->read(); + if(c == KISS_TFESC) { + c = KISS_FESC; + } else { + c = KISS_FEND; + } + } + kissBuffer[kissLen++] = c; + } + if(kissLen == 0 && c != KISS_FEND) { + if((c & 0xf) == 0) // First byte<3:0> should be a 0, otherwise we're having options + inFrame = true; + } + } + if(afsk.txReady()) { + radio->setModeTransmit(); + currentlySending = true; + if(!afsk.txStart()) { // Unable to start for some reason + radio->setModeReceive(); + currentlySending = false; + } + } + if(currentlySending && afsk.encoder.isDone()) { + radio->setModeReceive(); + currentlySending = false; + } +} + +void KISS::writePacket(AFSK::Packet *p) { + int i; + io->write(KISS_FEND); + io->write((uint8_t)0); // Host to TNC port identifier + for(i = 0; i < p->len-2; i++) { + char c = p->getByte(i); + if(c == KISS_FEND || c == KISS_FESC) { + io->write(KISS_FESC); + io->write((c==KISS_FEND?KISS_TFEND:KISS_TFESC)); + } else { + io->write(c); + } + } + io->write(KISS_FEND); +} diff --git a/src/KISS.h b/src/KISS.h new file mode 100644 index 0000000..aac88e6 --- /dev/null +++ b/src/KISS.h @@ -0,0 +1,36 @@ +#ifndef _KISS_H_ +#define _KISS_H_ + +#include +#include "packet.h" + +#define KISS_FEND 0xC0 +#define KISS_FESC 0xDB +#define KISS_TFEND 0xDC +#define KISS_TFESC 0xDD + +class KISS { +public: + KISS(Stream *_io, HamShield *h, DDS *d) : io(_io), radio(h), dds(d) {} + bool read(); + AFSK afsk; + void writePacket(AFSK::Packet *); + void loop(); + inline void isr() { + static uint8_t tcnt = 0; + TIFR1 = _BV(ICF1); // Clear the timer flag + dds->clockTick(); + if(++tcnt == (DDS_REFCLK_DEFAULT/9600)) { + //PORTD |= _BV(2); // Diagnostic pin (D2) + afsk.timer(); + tcnt = 0; + } + //PORTD &= ~(_BV(2)); + } +private: + Stream *io; + HamShield *radio; + DDS *dds; +}; + +#endif /* _KISS_H_ */ diff --git a/src/SimpleFIFO.h b/src/SimpleFIFO.h new file mode 100644 index 0000000..6965ce6 --- /dev/null +++ b/src/SimpleFIFO.h @@ -0,0 +1,89 @@ +#ifndef SimpleFIFO_h +#define SimpleFIFO_h +/* +|| +|| @file SimpleFIFO.h +|| @version 1.2 +|| @author Alexander Brevig +|| @contact alexanderbrevig@gmail.com +|| +|| @description +|| | A simple FIFO class, mostly for primitive types but can be used with classes if assignment to int is allowed +|| | This FIFO is not dynamic, so be sure to choose an appropriate size for it +|| # +|| +|| @license +|| | Copyright (c) 2010 Alexander Brevig +|| | This library is free software; you can redistribute it and/or +|| | modify it under the terms of the GNU Lesser General Public +|| | License as published by the Free Software Foundation; version +|| | 2.1 of the License. +|| | +|| | This library 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 +|| | Lesser General Public License for more details. +|| | +|| | You should have received a copy of the GNU Lesser General Public +|| | License along with this library; if not, write to the Free Software +|| | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +|| # +|| +*/ +template +class SimpleFIFO { +public: + const int size; //speculative feature, in case it's needed + + SimpleFIFO(); + + T dequeue(); //get next element + bool enqueue( T element ); //add an element + T peek() const; //get the next element without releasing it from the FIFO + void flush(); //[1.1] reset to default state + + //how many elements are currently in the FIFO? + unsigned char count() { return numberOfElements; } + +private: +#ifndef SimpleFIFO_NONVOLATILE + volatile unsigned char numberOfElements; + volatile unsigned char nextIn; + volatile unsigned char nextOut; + volatile T raw[rawSize]; +#else + unsigned char numberOfElements; + unsigned char nextIn; + unsigned char nextOut; + T raw[rawSize]; +#endif +}; + +template +SimpleFIFO::SimpleFIFO() : size(rawSize) { + flush(); +} +template +bool SimpleFIFO::enqueue( T element ) { + if ( count() >= rawSize ) { return false; } + numberOfElements++; + nextIn %= size; + raw[nextIn] = element; + nextIn++; //advance to next index + return true; +} +template +T SimpleFIFO::dequeue() { + numberOfElements--; + nextOut %= size; + return raw[ nextOut++]; +} +template +T SimpleFIFO::peek() const { + return raw[ nextOut % size]; +} +template +void SimpleFIFO::flush() { + nextIn = nextOut = numberOfElements = 0; +} +#endif diff --git a/src/packet.cpp b/src/packet.cpp new file mode 100644 index 0000000..d76a014 --- /dev/null +++ b/src/packet.cpp @@ -0,0 +1,738 @@ +#include +#include "SimpleFIFO.h" +#include "packet.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)) + +#ifdef PACKET_PREALLOCATE +SimpleFIFO preallocPool; +AFSK::Packet preallocPackets[PPOOL_SIZE]; +#endif + +void AFSK::Encoder::process() { + // We're on the start of a byte position, so fetch one + if(bitPosition == 0) { + if(preamble) { // Still in preamble + currentByte = HDLC_PREAMBLE; + --preamble; // Decrement by one + } else { + if(!packet) { // We aren't on a packet, grab one + // Unless we already sent enough + if(maxTx-- == 0) { + stop(); + lastTxEnd = millis(); + return; + } + packet = pBuf.getPacket(); + if(!packet) { // There actually weren't any + stop(); // Stop transmitting and return + lastTxEnd = millis(); + return; + } + lastTx = millis(); + currentBytePos = 0; + nextByte = HDLC_FRAME; // Our next output should be a frame boundary + hdlc = true; + } + + // We ran out of actual data, provide an HDLC frame (idle) + if(currentBytePos == packet->len && nextByte == 0) { + // We also get here if nextByte isn't set, to handle empty frames + pBuf.freePacket(packet); + packet = pBuf.getPacket(); // Get the next, if any + //packet = NULL; + currentBytePos = 0; + nextByte = 0; + currentByte = HDLC_FRAME; + hdlc = true; + } else { + if(nextByte) { + // We queued up something other than the actual stream to be sent next + currentByte = nextByte; + nextByte = 0; + } else { + // Get the next byte to send, but if it's an HDLC frame, escape it + // and queue the real byte for the next cycle. + currentByte = packet->getByte(); + if(currentByte == HDLC_FRAME) { + nextByte = currentByte; + currentByte = HDLC_ESCAPE; + } else { + currentBytePos++; + } + hdlc = false; // If we get here, it will be NRZI bit stuffed + } + } + } + } + + // Pickup the last bit + currentBit = currentByte & 0x1; + + if(lastZero == 5) { + currentBit = 0; // Force a 0 bit output + } else { + currentByte >>= 1; // Bit shift it right, for the next round + ++bitPosition; // Note our increase in position + } + + // To handle NRZI 5 bit stuffing, count the bits + if(!currentBit || hdlc) + lastZero = 0; + else + ++lastZero; + + // NRZI and AFSK uses toggling 0s, "no change" on 1 + // So, if not a 1, toggle to the opposite tone + if(!currentBit) + currentTone = !currentTone; + + if(currentTone == 0) { + PORTD |= _BV(7); + dds->setFrequency(AFSK_SPACE); + } else { + PORTD &= ~_BV(7); + dds->setFrequency(AFSK_MARK); + } +} + +bool AFSK::Encoder::start() { + if(!done || sending) { + return false; + } + + if(randomWait > millis()) { + return false; + } + + // First real byte is a frame + currentBit = 0; + lastZero = 0; + bitPosition = 0; + //bitClock = 0; + preamble = 0b110000; // 6.7ms each, 23 = 153ms + done = false; + hdlc = true; + packet = 0x0; // No initial packet, find in the ISR + currentBytePos = 0; + maxTx = 3; + sending = true; + nextByte = 0; + dds->setFrequency(0); + dds->on(); + return true; +} + +void AFSK::Encoder::stop() { + randomWait = 0; + sending = false; + done = true; + dds->setFrequency(0); + dds->off(); +} + +AFSK::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 ret = true; + + demod_bits <<= 1; + demod_bits |= bit ? 1 : 0; + + // Flag + if(demod_bits == HDLC_FRAME) { + fifo->enqueue(HDLC_FRAME); + rxstart = true; + currchar = 0; + bit_idx = 0; + return ret; + } + + // Reset + if((demod_bits & HDLC_RESET) == HDLC_RESET) { + rxstart = false; + lastRx = millis(); + return ret; + } + if(!rxstart) { + return ret; + } + + // Stuffed? + if((demod_bits & 0x3f) == 0x3e) + return ret; + + if(demod_bits & 0x01) + currchar |= 0x80; + + if(++bit_idx >= 8) { + if(currchar == HDLC_FRAME || + currchar == HDLC_RESET || + currchar == HDLC_ESCAPE) { + fifo->enqueue(HDLC_ESCAPE); + } + fifo->enqueue(currchar & 0xff); + currchar = 0; + bit_idx = 0; + } else { + currchar >>= 1; + } + + 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; + + // 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 + } +} + +// This routine uses a pre-allocated Packet structure +// to save on the memory requirements of the stream data +bool AFSK::Decoder::read() { + bool retVal = false; + if(!currentPacket) { // We failed a prior memory allocation + currentPacket = pBuf.makePacket(PACKET_MAX_LEN); + if(!currentPacket) // Still nothing + return false; + } + // While we have AFSK receive FIFO bytes... + while(rx_fifo.count()) { + // Grab the character + char c = rx_fifo.dequeue(); + bool escaped = false; + if(c == HDLC_ESCAPE) { // We received an escaped byte, mark it + escaped = true; + // Do we want to keep HDLC_ESCAPEs in the packet? + //currentPacket->append(HDLC_ESCAPE); // Append without FCS + c = rx_fifo.dequeue(); // Reset to the next character + } + + // Append all the bytes + // This will include unescaped HDLC_FRAME bytes + if(c != HDLC_FRAME || escaped) // Append frame if it is escaped + currentPacket->appendFCS(c); // Escaped characters and all else go into FCS + + if(currentPacket->len > PACKET_MAX_LEN) { + // We've now gone too far and picked up far too many bytes + // Cancel this frame, start back at the beginning + currentPacket->clear(); + continue; + } + + // We have a frame boundary, if it isn't escaped + // If it's escaped, it was part of the data stream + if(c == HDLC_FRAME && !escaped) { + if(!currentPacket->len) { + currentPacket->clear(); // There wasn't any data, restart stream + continue; + } else { + // We have some bytes in stream, check it meets minimum payload length + // Min payload is 1 (flag) + 14 (addressing) + 2 (control/PID) + 1 (flag) + if(currentPacket->len >= 16) { + // We should end up here with a valid FCS due to the appendFCS + if(currentPacket->crcOK()) { // Magic number for the CRC check passing + // Valid frame, so, let's filter for control + PID + // Maximum search distance is 71 bytes to end of the address fields + // Skip the HDLC frame start + bool filtered = false; + for(unsigned char i = 0; i < (currentPacket->len<70?currentPacket->len:71); ++i) { + if((currentPacket->getByte() & 0x1) == 0x1) { // Found a byte with LSB set + // which marks the final address payload + // next two bytes should be the control/PID + //if(currentPacket->getByte() == 0x03 && currentPacket->getByte() == 0xf0) { + filtered = true; + break; // Found it + //} + } + } + + if(!filtered) { + // Frame wasn't one we care about, discard + currentPacket->clear(); + continue; + } + + // It's all done and formatted, ready to go + currentPacket->ready = 1; + if(!pBuf.putPacket(currentPacket)) // Put it in the receive FIFO + pBuf.freePacket(currentPacket); // Out of FIFO space, so toss it + + // Allocate a new one of maximum length + currentPacket = pBuf.makePacket(PACKET_MAX_LEN); + retVal = true; + } + } + } + // Restart the stream + currentPacket->clear(); + } + } + return retVal; // This is true if we parsed a packet in this flow +} + +void AFSK::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; + // Configure the ADC and Timer1 to trigger automatic interrupts + TCCR1A = 0; + TCCR1B = _BV(CS11) | _BV(WGM13) | _BV(WGM12); + ICR1 = ((F_CPU / 8) / REFCLK) - 1; + ADMUX = _BV(REFS0) | _BV(ADLAR) | 0; // Channel 0, shift result left (ADCH used) + DDRC &= ~_BV(0); + PORTC &= ~_BV(0); + DIDR0 |= _BV(0); + ADCSRB = _BV(ADTS2) | _BV(ADTS1) | _BV(ADTS0); + ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIE) | _BV(ADPS2); // | _BV(ADPS0); */ +} + +AFSK::PacketBuffer::PacketBuffer() { + nextPacketIn = 0; + nextPacketOut = 0; + inBuffer = 0; + for(unsigned char i = 0; i < PACKET_BUFFER_SIZE; ++i) { + packets[i] = 0x0; + } +#ifdef PACKET_PREALLOCATE + for(unsigned char i = 0; i < PPOOL_SIZE; ++i) { + // Put some empty packets in the FIFO + preallocPool.enqueue(&preallocPackets[i]); + } +#endif +} + +unsigned char AFSK::PacketBuffer::readyCount() volatile { + unsigned char i; + unsigned int cnt = 0; + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { + for(i = 0; i < PACKET_BUFFER_SIZE; ++i) { + if(packets[i] && packets[i]->ready) + ++cnt; + } + } + return cnt; +} + +// Return NULL on empty packet buffers +AFSK::Packet *AFSK::PacketBuffer::getPacket() volatile { + unsigned char i = 0; + AFSK::Packet *p = NULL; + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { + if(inBuffer == 0) { + return 0x0; + } + + do { + p = packets[nextPacketOut]; + if(p) { + packets[nextPacketOut] = 0x0; + --inBuffer; + } + nextPacketOut = ++nextPacketOut % PACKET_BUFFER_SIZE; + ++i; + } while(!p && iinit(dlen); + } + return p; // Passes through a null on failure. +} + +// Free a packet struct, mainly convenience +void AFSK::PacketBuffer::freePacket(Packet *p) { + if(!p) + return; + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { +#ifdef PACKET_PREALLOCATE + preallocPool.enqueue(p); +#else + p->free(); + /*unsigned char i; + for(i = 0; i < PPOOL_SIZE; ++i) + if(p == &(pPool[i])) + break; + if(i < PPOOL_SIZE) + pStatus &= ~(1<= PACKET_BUFFER_SIZE) { + return false; + } + packets[nextPacketIn] = p; + nextPacketIn = ++nextPacketIn % PACKET_BUFFER_SIZE; + ++inBuffer; + } + return true; +} + +// Print a single byte to the data array +size_t AFSK::Packet::write(uint8_t c) { + return (appendFCS(c)?1:0); +} + +size_t AFSK::Packet::write(const uint8_t *ptr, size_t len) { + size_t i; + for(i = 0; i < len; ++i) + if(!appendFCS(ptr[i])) + break; + return i; +} + +// 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) { + uint8_t i; + for(i = 0; i < strlen(callsign) && i < 6; i++) { + appendFCS(callsign[i]<<1); + } + if(i < 6) { + for(;i<6;i++) { + appendFCS(' '<<1); + } + } + uint8_t ssidField = (ssid&0xf) << 1; + // TODO: Handle digis in the address C bit + if(final) { + ssidField |= 0b01100001; + } else { + ssidField |= 0b11100000; + } + appendFCS(ssidField); +} + +#ifdef PACKET_PARSER +// Process the AX25 frame and turn it into a bunch of useful strings +bool AFSK::Packet::parsePacket() { + uint8_t *d = dataPtr; + int i; + + // First 7 bytes are destination-ssid + for(i = 0; i < 6; i++) { + dstCallsign[i] = (*d++)>>1; + if(dstCallsign[i] == ' ') { + dstCallsign[i] = '\0'; + } + } + dstCallsign[6] = '\0'; + dstSSID = ((*d++)>>1) & 0xF; + + // Next 7 bytes are source-ssid + for(i = 0; i < 6; i++) { + srcCallsign[i] = (*d++)>>1; + if(srcCallsign[i] == ' ') { + srcCallsign[i] = '\0'; + } + } + srcCallsign[6] = '\0'; + srcSSID = *d++; // Don't shift yet, we need the LSB + + digipeater[0][0] = '\0'; // Set null in case we have none anyway + if((srcSSID & 1) == 0) { // Not the last address field + int digi; // Which digi we're on + for(digi = 0; digi < 8; digi++) { + for(i = 0; i < 6; i++) { + digipeater[digi][i] = (*d++)>>1; + if(digipeater[digi][i] == ' ') { + digipeater[digi][i] = '\0'; + } + } + uint8_t last = (*d) & 1; + digipeaterSSID[digi] = ((*d++)>>1) & 0xF; + if(last == 1) + break; + } + digipeater[digi][6] = '\0'; + for(digi += 1; digi<8; digi++) { // Empty out the rest of them + digipeater[digi][0] = '\0'; + } + } + + // Now handle the SSID itself + srcSSID >>= 1; + srcSSID &= 0xF; + + // After the address parsing, we end up on the control field + control = *d++; + // We have a PID if control type is U or I + // Control & 1 == 0 == I frame + // Control & 3 == 3 == U frame + if((control & 1) == 0 || (control & 3) == 3) + pid = *d++; + else pid = 0; + + // If there is no PID, we have no data + if(!pid) { + iFrameData = NULL; + return true; + } + + // At this point, we've walked far enough along that data is just at d + iFrameData = d; + + // Cheat a little by setting the first byte of the FCS to 0, making it a string + // First FCS byte is found at -2, HDLC flags aren't in this buffer + dataPtr[len-2] = '\0'; + + return true; +} +#endif + +void AFSK::Packet::printPacket(Stream *s) { + uint8_t i; +#ifdef PACKET_PARSER + if(!parsePacket()) { + s->print(F("Packet not valid")); + return; + } + + s->print(srcCallsign); + if(srcSSID > 0) { + s->write('-'); + s->print(srcSSID); + } + s->print(F(" > ")); + s->print(dstCallsign); + if(dstSSID > 0) { + s->write('-'); + s->print(dstSSID); + } + s->write(' '); + if(digipeater[0][0] != '\0') { + s->print(F("via ")); + for(i = 0; i < 8; i++) { + if(digipeater[i][0] == '\0') + break; + s->print(digipeater[i]); + if(digipeaterSSID[i] != 0) { + s->write('-'); + s->print(digipeaterSSID[i]); + } + if((digipeaterSSID[i] & _BV(7)) == _BV(7)) { + s->write('*'); // Digipeated already + } + // If we might have more, check to add a comma + if(i < 7 && digipeater[i+1][0] != '\0') { + s->write(','); + } + s->write(' '); + } + } + + // This is an S frame, we can only print control info + if(control & 3 == 1) { + switch((control>>2)&3) { + case 0: + s->print(F("RR")); + break; + case 1: + s->print(F("RNR")); + break; + case 2: + s->print(F("REJ")); + break; + case 3: // Undefined + s->print(F("unk")); + break; + } + // Use a + to indicate poll bit + if(control & _BV(4) == _BV(4)) { + s->write('+'); + } + } else if((control & 3) == 3) { // U Frame + s->print(F("U(")); + s->print(control, HEX); + s->write(','); + s->print(pid, HEX); + s->print(F(") ")); + } else if((control & 1) == 0) { // I Frame + s->print(F("I(")); + s->print(control, HEX); + s->write(','); + s->print(pid, HEX); + s->print(F(") ")); + } + s->print(F("len ")); + s->print(len); + s->print(F(": ")); + s->print((char *)iFrameData); + s->println(); +#else // no packet parser, do a rudimentary print + // Second 6 bytes are source callsign + for(i=7; i<13; i++) { + s->write(*(dataPtr+i)>>1); + } + // SSID + s->write('-'); + s->print((*(dataPtr+13) >> 1) & 0xF); + s->print(F(" -> ")); + // First 6 bytes are destination callsign + for(i=0; i<6; i++) { + s->write(*(dataPtr+i)>>1); + } + // SSID + s->write('-'); + s->print((*(dataPtr+6) >> 1) & 0xF); + // Control/PID next two bytes + // Skip those, print payload + for(i = 15; iwrite(*(dataPtr+i)); + } +#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))); + } +} + +void AFSK::start(DDS *dds) { + afskEnabled = true; + encoder.setDDS(dds); + decoder.start(); +} diff --git a/src/packet.h b/src/packet.h new file mode 100644 index 0000000..6f261d2 --- /dev/null +++ b/src/packet.h @@ -0,0 +1,302 @@ +#ifndef _AFSK_H_ +#define _AFSK_H_ + +#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 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; } + 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_ */