modem.cpp

C++ modem驱动代码

/**
 * modem.cpp
 * This file is part of the YATE Project http://YATE.null.ro
 *
 * Yet Another Modem
 *
 * Yet Another Telephony Engine - a fully featured software PBX and IVR
 * Copyright (C) 2004-2006 Null Team
 *
 * 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 2 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, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include "yatemodem.h"

#include <string.h>
#include <math.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

using namespace TelEngine;

// Amplitudes for the sine generator (mark and space) used to modulate data
#define MARK_AMPLITUDE  6300
#define SPACE_AMPLITUDE 6300

// Pattern length in ms to add after a modulated message
#define PATTERN_AFTER   2

// Uncomment this to view the bits decoded by the modem
#define YMODEM_BUFFER_BITS

// Constant values used by the FSK filter to modulate/demodulate data
class FilterConst
{
public:
    // Build constants used by this filter
    FilterConst(FSKModem::Type type);
    // Release memory
    ~FilterConst();
    // Calculate how many samples do we need to modulate n bits
    unsigned int bufLen(unsigned int nbits);
    // Get timing samples and advance the index
    inline unsigned int timingSamples(unsigned int& index) {
	    unsigned int tmp = bitSamples[index];
	    if (++index == bitSamplesLen)
		index = 0;
	    return tmp;
	}

    // Signal properties
    float markFreq;                      // Mark frequency
    float spaceFreq;                     // Space frequency
    float sampleRate;                    // Sampling rate
    float baudRate;                      // Transmission baud rate (bps)
    // Modulation/demodulation data
    double markCoef;                     // Mark coefficient
    double spaceCoef;                    // Space coefficient
    // Data used to demodulate signals
    unsigned int spb;                    // The number of samples per bit (also the length of all buffers)
    unsigned int halfSpb;                // Half of the spb value (used to filter data)
    float bitLen;                        // The exact value of bit length in samples
    float halfBitLen;                    // Half the bit length
    float markGain;
    float spaceGain;
    float lowbandGain;
    float* mark;
    float* space;
    float* lowband;
    // Data used to modulate signals
    double accSin;                       // Accumulate sine radians during modulation
                                         // This value is updated after the header is modulated
                                         // and used as start value for each message data
    unsigned int* bitSamples;            // Array of bit samples nedded to maintain the modulation timing
    unsigned int bitSamplesLen;          // The length of the bitSamples array
    DataBlock header;                    // Modulated message header
                                         // e.g. ETSI: channel seizure pattern + marks
};

struct FilterData
{
    ~FilterData() {
	    if (xbuf)
		delete[] xbuf;
	    if (ybuf)
		delete[] ybuf;
	}

    inline void init(unsigned int len) {
	    xbuf = new float[len];
	    ybuf = new float[len];
	    ::memset(xbuf,0,len*sizeof(float));
	    ::memset(ybuf,0,len*sizeof(float));
	}

    float* xbuf;
    float* ybuf;
};


namespace TelEngine {

// BitBuffer
class BitBuffer
{
public:
    inline BitBuffer()
	: m_accumulator(8)
	{}
    inline const DataBlock& buffer() const
	{ return m_buffer; }
    inline void reset() {
	    m_buffer.clear();
	    m_accumulator.reset();
	}
    // Accumulate a bit. Add data bytes to buffer once accumulated
    inline bool accumulate(bool bit) {
#ifdef YMODEM_BUFFER_BITS
	    unsigned int res = m_accumulator.accumulate(bit);
	    if (res > 255)
		return false;
	    unsigned char c = (unsigned char)res;
	    DataBlock tmp(&c,1,false);
	    m_buffer += tmp;
	    tmp.clear(false);
#endif
	    return true;
	}
    // Operator used to accumulate a bit
    inline BitBuffer& operator+=(bool bit)
	{ accumulate(bit); return *this; }
    // Print bits to output
    void printBits(DebugEnabler* dbg, unsigned int linelen = 80);
private:
    DataBlock m_buffer;                  // The data byte buffer
    BitAccumulator m_accumulator;        // The bit accumulator
};

// The FSK sample filter
class FSKFilter
{
public:
    FSKFilter(int type);
    // Get the constants used by this filter
    inline FilterConst* constants()
	{ return m_const; }
    // Check if FSK modulation was already detected
    inline bool fskStarted() const
	{ return m_fskStarted > 0; }
    // Process data to demodulate a bit
    // Return negative if buffer ended, 0/1 if found a bit
    int getBit(short*& samples, unsigned int& len);
    // Filter data until a start bit is found (used to wait for FSK modulation to start)
    // Return true if a start bit is found, false if all buffer was processed with no result
    bool waitFSK(short*& samples, unsigned int& len);
    // Add a modulated bit to a destination buffer. Advance the buffer's index
    void addBit(short* samples, unsigned int& index, bool bit);
    // Add a modulated data byte to a destination buffer
    // dataBits must not be 0 or greater the then 8
    inline void addByte(short* samples, unsigned int& index,
	unsigned char value, unsigned char dataBits) {
	    for (unsigned int i = 0; i < dataBits; i++, value >>= 1)
		addBit(samples,index,(bool)(value & 0x01));
	}
    // Add a complete modulated byte to a destination buffer
    // The data is enclosed by start/stop bits
    inline void addByteFull(short* samples, unsigned int& index,
	unsigned char value, unsigned char dataBits) {
	    addBit(samples,index,false);
	    addByte(samples,index,value,dataBits);
	    addBit(samples,index,true);
	}
    // Modulate data to a buffer. Reset the destination's length
    // dataBits must not be 0 or greater then 8
    // Returns the current sine accumulator value
    double addBuffer(DataBlock& dest, const DataBlock& src, unsigned char dataBits, bool full);
private:
    // Apply mark, space and low band filter
    float filter(short*& samples, unsigned int& len);

    int m_fskStarted;                    // Flag indicating the FSK modulation start
    float m_lastFiltered;                // The last result of the filter
    float m_processed;                   // How much of a bit length was processed in (this is used for clock recovery)
    unsigned int m_index;                // Current index in buffer
    FilterConst* m_const;                // Constants used by this filter
    FilterData m_mark;
    FilterData m_space;
    FilterData m_lowband;
    // Data use to modulate signals
    double m_accSin;                     // Accumulate sine radians during modulation
    unsigned int m_bitSamples;           // Current index in the filter constant's bitSamples array
};

}


/**
 * Static module data
 */
static const char* s_libName = "libyatemodem";

FilterConst s_filterConst[FSKModem::TypeCount] = {
    FilterConst(FSKModem::ETSI)
};


/**
 * FilterConst
 */
// Build constants used by this filter
FilterConst::FilterConst(FSKModem::Type type)
{
    static float m[7] = {-5.6297236492e-02, 4.2915323820e-01, -1.2609358633e+00, 2.2399213250e+00,
		         -2.9928879142e+00, 2.5990173742e+00, 0.0000000000e+00};
    static float s[7] = {-5.6297236492e-02, -1.1421579050e-01, -4.8122536483e-01, -4.0121072432e-01,
		         -7.4834487567e-01, -6.9170822332e-01, 0.0000000000e+00};
    static float l[7] = {-7.8390522307e-03, 8.5209627801e-02, -4.0804129163e-01, 1.1157139955e+00, 
		         -1.8767603680e+00, 1.8916395224e+00, 0.0000000000e+00};

    switch (type) {
	case FSKModem::ETSI:
	    break;
	default:
	    ::memset(this,0,sizeof(*this));
	    return;
    }

    // ETSI

    // Signal properties
    markFreq = 1200.0;
    spaceFreq = 2200.0;
    sampleRate = 8000.0;
    baudRate = 1200.0;

    // Mark/space coefficients for modulation/demodulation
    markCoef = 2 * M_PI * markFreq / sampleRate;
    spaceCoef = 2 * M_PI * spaceFreq / sampleRate;

    spb = 7;
    halfSpb = spb / 2;
    bitLen = sampleRate / baudRate;
    halfBitLen = bitLen / 2;
    markGain = 9.8539686961e-02;
    spaceGain = 9.8531161839e-02;
    lowbandGain = 3.1262119724e-03;
    mark = new float[spb+1];
    space = new float[spb+1];
    lowband = new float[spb+1];

    for (unsigned int i = 0; i < spb; i++) {
	mark[i] = m[i];
	space[i] = s[i];
	lowband[i] = l[i];
    }

    // Build the array of bit samples nedded to maintain the modulation timing
    bitSamplesLen = 3;
    bitSamples = new unsigned int[bitSamplesLen];
    bitSamples[0] = bitSamples[2] = 7;
    bitSamples[1] = 6;

    accSin = 0;
    // Build message header
    // ETSI channel seizure signal + Mark (stop bits) signal
    // 300 continuous bits of alternating 0 and 1 + 180 of 1 (mark) bits
    // 480 bits: 60 bytes. Byte 38: 01011111
    // This is the data header to be sent with ETSI messages
    unsigned char* hdr = new unsigned char[60];
    ::memset(hdr,0x55,37);
    hdr[37] = 0xf5;
    ::memset(&hdr[38],0xff,22);
    DataBlock src;
    FSKModem::addRaw(src,hdr,60);
    FSKFilter filter(type);
    // Keep the sine accumulator to be used when modulating data
    accSin = filter.addBuffer(header,src,8,false);

    Debug(s_libName,DebugInfo,"Initialized filter tables for type '%s' headerlen=%u",
	lookup(FSKModem::ETSI,FSKModem::s_typeName),header.length());
}

// Release memory
FilterConst::~FilterConst()
{
    if (!spb)
	return;
    delete[] mark;
    delete[] space;
    delete[] lowband;
    delete[] bitSamples;
}

// Calculate how many samples do we need to modulate n bits
unsigned int FilterConst::bufLen(unsigned int n)
{
    if (!bitSamples)
	return 0;
    unsigned int count = 0;
    // Each entry in bitSamples contain the number of samples nedded for current bit
    for (unsigned int idx = 0; n; n--)
	count += timingSamples(idx);
    return count;
}

/**
 * BitBuffer
 */
void BitBuffer::printBits(DebugEnabler* dbg, unsigned int linelen)
{
#ifdef YMODEM_BUFFER_BITS
    if ((dbg && !dbg->debugAt(DebugAll)) || (!dbg && !TelEngine::debugAt(DebugAll)))
	return;

    ObjList lines;
    String* s = new String;
    unsigned char* p = (unsigned char*)m_buffer.data();
    for (unsigned int i = 0; i < m_buffer.length(); i++, p++) {
	for (unsigned char pos = 0; pos < 8; pos++) {
	    char c = (*p & (1 << pos)) ? '1' : '0';
	    *s += c;
	}
	if (s->length() == linelen) {
	    lines.append(s);
	    s = new String;
	}
    }
    if (s->length())
	lines.append(s);
    else
	TelEngine::destruct(s);
    String tmp;
    for (ObjList* o = lines.skipNull(); o; o = o->skipNext())
	tmp << "\r\n" << *(static_cast<String*>(o->get()));
    Debug(dbg,DebugAll,"Decoded %u bits:%s",m_buffer.length()*8,tmp.c_str());
#endif
}