Big enhancements:
Decryption support (DES-OFB) Fixes to GRC files Optional logging to console Optional disabling of output silence during idle time More error correction Decoding of LSDW BCH error correction fix (after IT++ BCH breaking change) Fix for GR 3.7 build process
This commit is contained in:
parent
2051a96198
commit
aea5479646
|
@ -83,7 +83,6 @@ set(GRC_BLOCKS_DIR ${GR_PKG_DATA_DIR}/grc/blocks)
|
|||
########################################################################
|
||||
# Find gnuradio build dependencies
|
||||
########################################################################
|
||||
find_package(GnuradioRuntime)
|
||||
find_package(CppUnit)
|
||||
|
||||
# To run a more advanced search for GNU Radio and it's components and
|
||||
|
@ -91,8 +90,9 @@ find_package(CppUnit)
|
|||
# of GR_REQUIRED_COMPONENTS (in all caps) and change "version" to the
|
||||
# minimum API compatible version required.
|
||||
#
|
||||
# set(GR_REQUIRED_COMPONENTS RUNTIME BLOCKS FILTER ...)
|
||||
set(GR_REQUIRED_COMPONENTS RUNTIME BLOCKS FILTER PMT)
|
||||
#find_package(Gnuradio "version")
|
||||
find_package(Gnuradio)
|
||||
|
||||
if(NOT GNURADIO_RUNTIME_FOUND)
|
||||
message(FATAL_ERROR "GnuRadio Runtime required to compile op25")
|
||||
|
|
|
@ -4,17 +4,17 @@
|
|||
<key>op25_decoder_ff</key>
|
||||
<category>op25</category>
|
||||
<import>import op25</import>
|
||||
<make>op25.decoder_ff($)</make>
|
||||
<make>op25.decoder_ff()</make>
|
||||
<!-- Make one 'param' node for every Parameter you want settable from the GUI.
|
||||
Sub-nodes:
|
||||
* name
|
||||
* key (makes the value accessible as $keyname, e.g. in the make node)
|
||||
* type -->
|
||||
<param>
|
||||
<!--<param>
|
||||
<name>...</name>
|
||||
<key>...</key>
|
||||
<type>...</type>
|
||||
</param>
|
||||
</param>-->
|
||||
|
||||
<!-- Make one 'sink' node per input. Sub-nodes:
|
||||
* name (an identifier for the GUI)
|
||||
|
@ -23,7 +23,7 @@
|
|||
* optional (set to 1 for optional inputs) -->
|
||||
<sink>
|
||||
<name>in</name>
|
||||
<type><!-- e.g. int, float, complex, byte, short, xxx_vector, ...--></type>
|
||||
<type>float</type>
|
||||
</sink>
|
||||
|
||||
<!-- Make one 'source' node per output. Sub-nodes:
|
||||
|
@ -32,7 +32,7 @@
|
|||
* vlen
|
||||
* optional (set to 1 for optional inputs) -->
|
||||
<source>
|
||||
<name>out</name>
|
||||
<type><!-- e.g. int, float, complex, byte, short, xxx_vector, ...--></type>
|
||||
<name>audio</name>
|
||||
<type>float</type>
|
||||
</source>
|
||||
</block>
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
<key>op25_fsk4_demod_ff</key>
|
||||
<category>op25</category>
|
||||
<import>import op25</import>
|
||||
<make>op25.fsk4_demod_ff(self.auto_tune_msgq, $sample_rate, $symbol_rate)</make>
|
||||
<make>op25.fsk4_demod_ff($(id)_msgq_out, $sample_rate, $symbol_rate)</make>
|
||||
|
||||
<param>
|
||||
<name>Sample Rate</name>
|
||||
|
@ -20,7 +20,8 @@
|
|||
<type>real</type>
|
||||
</param>
|
||||
|
||||
<param>
|
||||
<!-- Must connect tune message queue as the block expects a queue as first argument! -->
|
||||
<!--<param>
|
||||
<name>Output Auto Tune</name>
|
||||
<key>tune_out</key>
|
||||
<value>True</value>
|
||||
|
@ -34,7 +35,7 @@
|
|||
<name>No</name>
|
||||
<key>False</key>
|
||||
</option>
|
||||
</param>
|
||||
</param>-->
|
||||
|
||||
<sink>
|
||||
<name>in</name>
|
||||
|
@ -45,4 +46,10 @@
|
|||
<name>dibits</name>
|
||||
<type>float</type>
|
||||
</source>
|
||||
|
||||
<source>
|
||||
<name>tune</name>
|
||||
<type>msg</type>
|
||||
<!--<optional>1</optional>-->
|
||||
</source>
|
||||
</block>
|
||||
|
|
|
@ -51,7 +51,7 @@ namespace gr {
|
|||
* class. op25::decoder_bf::make is the public interface for
|
||||
* creating new instances.
|
||||
*/
|
||||
static sptr make();
|
||||
static sptr make(bool idle_silence = true, bool verbose = false);
|
||||
|
||||
/**
|
||||
* Return a pointer to a string identifying the destination of
|
||||
|
@ -78,6 +78,17 @@ namespace gr {
|
|||
* message queue.
|
||||
*/
|
||||
virtual void set_msgq(gr::msg_queue::sptr msgq) = 0;
|
||||
|
||||
virtual void set_idle_silence(bool idle_silence = true) = 0;
|
||||
|
||||
virtual void set_logging(bool verbose = true) = 0;
|
||||
|
||||
typedef std::vector<uint8_t> key_type;
|
||||
typedef std::map<uint16_t, key_type> key_map_type;
|
||||
|
||||
virtual void set_key(const key_type& key) = 0;
|
||||
|
||||
virtual void set_key_map(const key_map_type& keys) = 0;
|
||||
};
|
||||
|
||||
} // namespace op25
|
||||
|
|
|
@ -53,6 +53,13 @@ list(APPEND op25_sources
|
|||
value_string.cc
|
||||
pickle.cc
|
||||
pcap_source_b_impl.cc
|
||||
bch.cc
|
||||
ldu.cc
|
||||
crypto.cc
|
||||
crypto_module_du_handler.cc
|
||||
deskey.c
|
||||
desport.c
|
||||
dessp.c
|
||||
)
|
||||
|
||||
add_library(gnuradio-op25 SHARED ${op25_sources})
|
||||
|
|
|
@ -55,10 +55,10 @@ abstract_data_unit::correct_errors()
|
|||
}
|
||||
|
||||
void
|
||||
abstract_data_unit::decode_audio(imbe_decoder& imbe)
|
||||
abstract_data_unit::decode_audio(imbe_decoder& imbe, crypto_module::sptr crypto_mod)
|
||||
{
|
||||
if(is_complete()) {
|
||||
do_decode_audio(d_frame_body, imbe);
|
||||
do_decode_audio(d_frame_body, imbe, crypto_mod);
|
||||
} else {
|
||||
ostringstream msg;
|
||||
msg << "cannot decode audio - frame is not complete" << endl;
|
||||
|
@ -153,7 +153,8 @@ abstract_data_unit::dump(ostream& os) const
|
|||
}
|
||||
|
||||
abstract_data_unit::abstract_data_unit(const_bit_queue& frame_body) :
|
||||
d_frame_body(frame_body.size())
|
||||
d_frame_body(frame_body.size()),
|
||||
d_logging_enabled(false)
|
||||
{
|
||||
copy(frame_body.begin(), frame_body.end(), d_frame_body.begin());
|
||||
}
|
||||
|
@ -164,7 +165,7 @@ abstract_data_unit::do_correct_errors(bit_vector& frame_body)
|
|||
}
|
||||
|
||||
void
|
||||
abstract_data_unit::do_decode_audio(const_bit_vector& frame_body, imbe_decoder& imbe)
|
||||
abstract_data_unit::do_decode_audio(const_bit_vector& frame_body, imbe_decoder& imbe, crypto_module::sptr crypto_mod)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -179,3 +180,15 @@ abstract_data_unit::frame_size() const
|
|||
{
|
||||
return d_frame_body.size();
|
||||
}
|
||||
|
||||
void
|
||||
abstract_data_unit::set_logging(bool on)
|
||||
{
|
||||
d_logging_enabled = on;
|
||||
}
|
||||
|
||||
bool
|
||||
abstract_data_unit::logging_enabled() const
|
||||
{
|
||||
return d_logging_enabled;
|
||||
}
|
||||
|
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
#include "data_unit.h"
|
||||
#include "op25_yank.h"
|
||||
#include "crypto.h"
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
@ -62,7 +63,7 @@ public:
|
|||
* \precondition is_complete() == true.
|
||||
* \param imbe The imbe_decoder to use to generate the audio.
|
||||
*/
|
||||
virtual void decode_audio(imbe_decoder& imbe);
|
||||
virtual void decode_audio(imbe_decoder& imbe, crypto_module::sptr crypto_mod);
|
||||
|
||||
/**
|
||||
* Decode the frame into an octet vector.
|
||||
|
@ -117,6 +118,15 @@ public:
|
|||
*/
|
||||
virtual std::string snapshot() const;
|
||||
|
||||
/**
|
||||
* Returns a string describing the Data Unit ID (DUID).
|
||||
*
|
||||
* \return A string identifying the DUID.
|
||||
*/
|
||||
virtual std::string duid_str() const = 0;
|
||||
|
||||
virtual void set_logging(bool on);
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
|
@ -140,7 +150,7 @@ protected:
|
|||
* \param frame_body The const_bit_vector to decode.
|
||||
* \param imbe The imbe_decoder to use.
|
||||
*/
|
||||
virtual void do_decode_audio(const_bit_vector& frame_body, imbe_decoder& imbe);
|
||||
virtual void do_decode_audio(const_bit_vector& frame_body, imbe_decoder& imbe, crypto_module::sptr crypto_mod);
|
||||
|
||||
/**
|
||||
* Decode frame_body and write the decoded frame contents to msg.
|
||||
|
@ -152,13 +162,6 @@ protected:
|
|||
*/
|
||||
virtual size_t decode_frame(const_bit_vector& frame_body, size_t msg_sz, uint8_t *msg);
|
||||
|
||||
/**
|
||||
* Returns a string describing the Data Unit ID (DUID).
|
||||
*
|
||||
* \return A string identifying the DUID.
|
||||
*/
|
||||
virtual std::string duid_str() const = 0;
|
||||
|
||||
/**
|
||||
* Return a reference to the frame body.
|
||||
*/
|
||||
|
@ -180,6 +183,8 @@ protected:
|
|||
*/
|
||||
virtual uint16_t frame_size() const;
|
||||
|
||||
virtual bool logging_enabled() const;
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
|
@ -187,6 +192,7 @@ private:
|
|||
*/
|
||||
bit_vector d_frame_body;
|
||||
|
||||
bool d_logging_enabled;
|
||||
};
|
||||
|
||||
#endif /* INCLUDED_ABSTRACT_DATA_UNIT_H */
|
||||
|
|
|
@ -0,0 +1,162 @@
|
|||
|
||||
#include <stdio.h>
|
||||
#include <vector>
|
||||
#include "bch.h"
|
||||
/*
|
||||
* Copyright 2010, KA1RBI
|
||||
*/
|
||||
static const int bchGFexp[64] = {
|
||||
1, 2, 4, 8, 16, 32, 3, 6, 12, 24, 48, 35, 5, 10, 20, 40,
|
||||
19, 38, 15, 30, 60, 59, 53, 41, 17, 34, 7, 14, 28, 56, 51, 37,
|
||||
9, 18, 36, 11, 22, 44, 27, 54, 47, 29, 58, 55, 45, 25, 50, 39,
|
||||
13, 26, 52, 43, 21, 42, 23, 46, 31, 62, 63, 61, 57, 49, 33, 0
|
||||
};
|
||||
|
||||
static const int bchGFlog[64] = {
|
||||
-1, 0, 1, 6, 2, 12, 7, 26, 3, 32, 13, 35, 8, 48, 27, 18,
|
||||
4, 24, 33, 16, 14, 52, 36, 54, 9, 45, 49, 38, 28, 41, 19, 56,
|
||||
5, 62, 25, 11, 34, 31, 17, 47, 15, 23, 53, 51, 37, 44, 55, 40,
|
||||
10, 61, 46, 30, 50, 22, 39, 43, 29, 60, 42, 21, 20, 59, 57, 58
|
||||
};
|
||||
|
||||
static const int bchG[48] = {
|
||||
1, 1, 0, 1, 0, 1, 0, 0, 1, 1, 0, 1, 1, 1, 0, 0,
|
||||
1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0,
|
||||
1, 1, 0, 0, 1, 0, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1
|
||||
};
|
||||
|
||||
int bchDec(bit_vector& Codeword)
|
||||
{
|
||||
|
||||
int elp[24][ 22], S[23];
|
||||
int D[23], L[24], uLu[24];
|
||||
int root[11], locn[11], reg[12];
|
||||
int i,j,U,q,count;
|
||||
int SynError, CantDecode;
|
||||
|
||||
SynError = 0; CantDecode = 0;
|
||||
|
||||
for(i = 1; i <= 22; i++) {
|
||||
S[i] = 0;
|
||||
// FOR j = 0 TO 62
|
||||
for(j = 0; j <= 62; j++) {
|
||||
if( Codeword[j]) { S[i] = S[i] ^ bchGFexp[(i * j) % 63]; }
|
||||
}
|
||||
if( S[i]) { SynError = 1; }
|
||||
S[i] = bchGFlog[S[i]];
|
||||
// printf("S[%d] %d\n", i, S[i]);
|
||||
}
|
||||
|
||||
if( SynError) { //if there are errors, try to correct them
|
||||
L[0] = 0; uLu[0] = -1; D[0] = 0; elp[0][ 0] = 0;
|
||||
L[1] = 0; uLu[1] = 0; D[1] = S[1]; elp[1][ 0] = 1;
|
||||
//FOR i = 1 TO 21
|
||||
for(i = 1; i <= 21; i++) {
|
||||
elp[0][ i] = -1; elp[1][ i] = 0;
|
||||
}
|
||||
U = 0;
|
||||
|
||||
do {
|
||||
U = U + 1;
|
||||
if( D[U] == -1) {
|
||||
L[U + 1] = L[U];
|
||||
// FOR i = 0 TO L[U]
|
||||
for(i = 0; i <= L[U]; i++) {
|
||||
elp[U + 1][ i] = elp[U][ i]; elp[U][ i] = bchGFlog[elp[U][ i]];
|
||||
}
|
||||
} else {
|
||||
//search for words with greatest uLu(q) for which d(q)!=0
|
||||
q = U - 1;
|
||||
while((D[q] == -1) &&(q > 0)) { q = q - 1; }
|
||||
//have found first non-zero d(q)
|
||||
if( q > 0) {
|
||||
j = q;
|
||||
do { j = j - 1; if((D[j] != -1) &&(uLu[q] < uLu[j])) { q = j; }
|
||||
} while( j > 0) ;
|
||||
}
|
||||
|
||||
//store degree of new elp polynomial
|
||||
if( L[U] > L[q] + U - q) {
|
||||
L[U + 1] = L[U] ;
|
||||
} else {
|
||||
L[U + 1] = L[q] + U - q;
|
||||
}
|
||||
|
||||
///* form new elp(x) */
|
||||
// FOR i = 0 TO 21
|
||||
for(i = 0; i <= 21; i++) {
|
||||
elp[U + 1][ i] = 0;
|
||||
}
|
||||
// FOR i = 0 TO L(q)
|
||||
for(i = 0; i <= L[q]; i++) {
|
||||
if( elp[q][ i] != -1) {
|
||||
elp[U + 1][ i + U - q] = bchGFexp[(D[U] + 63 - D[q] + elp[q][ i]) % 63];
|
||||
}
|
||||
}
|
||||
// FOR i = 0 TO L(U)
|
||||
for(i = 0; i <= L[U]; i++) {
|
||||
elp[U + 1][ i] = elp[U + 1][ i] ^ elp[U][ i];
|
||||
elp[U][ i] = bchGFlog[elp[U][ i]];
|
||||
}
|
||||
}
|
||||
uLu[U + 1] = U - L[U + 1];
|
||||
|
||||
//form(u+1)th discrepancy
|
||||
if( U < 22) {
|
||||
//no discrepancy computed on last iteration
|
||||
if( S[U + 1] != -1) { D[U + 1] = bchGFexp[S[U + 1]]; } else { D[U + 1] = 0; }
|
||||
// FOR i = 1 TO L(U + 1)
|
||||
for(i = 1; i <= L[U + 1]; i++) {
|
||||
if((S[U + 1 - i] != -1) &&(elp[U + 1][ i] != 0)) {
|
||||
D[U + 1] = D[U + 1] ^ bchGFexp[(S[U + 1 - i] + bchGFlog[elp[U + 1][ i]]) % 63];
|
||||
}
|
||||
}
|
||||
//put d(u+1) into index form */
|
||||
D[U + 1] = bchGFlog[D[U + 1]];
|
||||
}
|
||||
} while((U < 22) &&(L[U + 1] <= 11));
|
||||
|
||||
U = U + 1;
|
||||
if( L[U] <= 11) { // /* Can correct errors */
|
||||
//put elp into index form
|
||||
// FOR i = 0 TO L[U]
|
||||
for(i = 0; i <= L[U]; i++) {
|
||||
elp[U][ i] = bchGFlog[elp[U][ i]];
|
||||
}
|
||||
|
||||
//Chien search: find roots of the error location polynomial
|
||||
// FOR i = 1 TO L(U)
|
||||
for(i = 1; i <= L[U]; i++) {
|
||||
reg[i] = elp[U][ i];
|
||||
}
|
||||
count = 0;
|
||||
// FOR i = 1 TO 63
|
||||
for(i = 1; i <= 63; i++) {
|
||||
q = 1;
|
||||
//FOR j = 1 TO L(U)
|
||||
for(j = 1; j <= L[U]; j++) {
|
||||
if( reg[j] != -1) {
|
||||
reg[j] =(reg[j] + j) % 63; q = q ^ bchGFexp[reg[j]];
|
||||
}
|
||||
}
|
||||
if( q == 0) { //store root and error location number indices
|
||||
root[count] = i; locn[count] = 63 - i; count = count + 1;
|
||||
}
|
||||
}
|
||||
if( count == L[U]) {
|
||||
//no. roots = degree of elp hence <= t errors
|
||||
//FOR i = 0 TO L[U] - 1
|
||||
for(i = 0; i <= L[U]-1; i++) {
|
||||
Codeword[locn[i]] = Codeword[locn[i]] ^ 1;
|
||||
}
|
||||
CantDecode = count;
|
||||
} else { //elp has degree >t hence cannot solve
|
||||
CantDecode = -1;
|
||||
}
|
||||
} else {
|
||||
CantDecode = -2;
|
||||
}
|
||||
}
|
||||
return CantDecode;
|
||||
}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
#include <vector>
|
||||
typedef std::vector<bool> bit_vector;
|
||||
int bchDec(bit_vector& Codeword);
|
||||
|
|
@ -0,0 +1,284 @@
|
|||
#include "crypto.h"
|
||||
|
||||
#include <sstream>
|
||||
#include <boost/format.hpp>
|
||||
#include <iostream>
|
||||
|
||||
extern "C" {
|
||||
#include "des.h"
|
||||
}
|
||||
|
||||
static unsigned long long swap_bytes(uint64_t l)
|
||||
{
|
||||
unsigned long long r;
|
||||
unsigned char* pL = (unsigned char*)&l;
|
||||
unsigned char* pR = (unsigned char*)&r;
|
||||
for (int i = 0; i < sizeof(l); ++i)
|
||||
pR[i] = pL[(sizeof(l) - 1) - i];
|
||||
return r;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
/*
|
||||
class null_algorithm : public crypto_algorithm // This is an algorithm skeleton (can be used for no encryption as pass-through)
|
||||
{
|
||||
private:
|
||||
size_t m_generated_bits;
|
||||
public:
|
||||
null_algorithm()
|
||||
: m_generated_bits(0)
|
||||
{
|
||||
}
|
||||
const type_id id() const
|
||||
{
|
||||
return crypto_algorithm::NONE;
|
||||
}
|
||||
bool update(const struct CryptoState& state)
|
||||
{
|
||||
fprintf(stderr, "NULL:\t%d bits generated\n", m_generated_bits);
|
||||
|
||||
m_generated_bits = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
bool set_key(const crypto_algorithm::key_type& key)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
uint64_t generate(size_t n)
|
||||
{
|
||||
m_generated_bits += n;
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
*/
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class des_ofb : public crypto_algorithm
|
||||
{
|
||||
public:
|
||||
unsigned long long m_key_des, m_next_iv, m_ks;
|
||||
int m_ks_idx;
|
||||
DES_KS m_ksDES;
|
||||
int m_iterations;
|
||||
uint16_t m_current_kid;
|
||||
key_type m_default_key;
|
||||
key_map_type m_key_map;
|
||||
bool m_verbose;
|
||||
public:
|
||||
des_ofb()
|
||||
: m_current_kid(-1)
|
||||
{
|
||||
memset(&m_ksDES, 0, sizeof(m_ksDES));
|
||||
m_key_des = 0;
|
||||
m_next_iv = 0;
|
||||
m_ks_idx = 0;
|
||||
m_ks = 0;
|
||||
m_iterations = 0;
|
||||
}
|
||||
|
||||
void set_logging(bool on)
|
||||
{
|
||||
m_verbose = on;
|
||||
}
|
||||
|
||||
const type_id id() const
|
||||
{
|
||||
return crypto_algorithm::DES_OFB;
|
||||
}
|
||||
|
||||
bool update(const struct CryptoState& state)
|
||||
{
|
||||
if (m_current_kid != state.kid)
|
||||
{
|
||||
if (m_key_map.empty())
|
||||
{
|
||||
// Nothing to do
|
||||
}
|
||||
else
|
||||
{
|
||||
key_map_type::iterator it = m_key_map.find(state.kid);
|
||||
if (it != m_key_map.end())
|
||||
{
|
||||
set_key(it->second);
|
||||
}
|
||||
else if (!m_default_key.empty())
|
||||
{
|
||||
/*if (m_verbose) */fprintf(stderr, "Key 0x%04x not found in key map - using default key\n", state.kid);
|
||||
|
||||
set_key(m_default_key);
|
||||
}
|
||||
else
|
||||
{
|
||||
/*if (m_verbose) */fprintf(stderr, "Key 0x%04x not found in key map and no default key\n", state.kid);
|
||||
}
|
||||
}
|
||||
|
||||
m_current_kid = state.kid;
|
||||
}
|
||||
|
||||
uint64_t iv = 0;
|
||||
size_t n = std::min(sizeof(iv), state.mi.size());
|
||||
memcpy(&iv, &state.mi[0], n);
|
||||
set_iv(iv);
|
||||
|
||||
return (n == 8);
|
||||
}
|
||||
|
||||
void set_key_map(const key_map_type& key_map)
|
||||
{
|
||||
m_key_map = key_map;
|
||||
|
||||
m_current_kid = -1; // To refresh on next update if it has changed
|
||||
}
|
||||
|
||||
bool set_key(const crypto_algorithm::key_type& key)
|
||||
{
|
||||
const size_t valid_key_length = 8;
|
||||
|
||||
if (key.size() != valid_key_length)
|
||||
{
|
||||
if (m_verbose) fprintf(stderr, "DES:\tIncorrect key length of %lu (should be %lu)\n", key.size(), valid_key_length);
|
||||
return false;
|
||||
}
|
||||
|
||||
m_default_key = key;
|
||||
|
||||
memcpy(&m_key_des, &key[0], std::min(key.size(), sizeof(m_key_des)));
|
||||
|
||||
if (m_verbose)
|
||||
{
|
||||
std::stringstream ss;
|
||||
for (int i = 0; i < valid_key_length; ++i)
|
||||
ss << boost::format("%02X") % (int)key[i];
|
||||
std::cerr << "DES:\tKey: " << ss.str() << std::endl;
|
||||
}
|
||||
|
||||
deskey(m_ksDES, (unsigned char*)&m_key_des, 0); // 0: encrypt (for OFB mode)
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void set_iv(uint64_t iv)
|
||||
{
|
||||
if (m_iterations > 0)
|
||||
{
|
||||
if (m_verbose) fprintf(stderr, "DES:\t%i bits used from %i iterations\n", m_ks_idx, m_iterations);
|
||||
}
|
||||
|
||||
m_next_iv = iv;
|
||||
|
||||
m_ks_idx = 0;
|
||||
m_iterations = 0;
|
||||
|
||||
m_ks = m_next_iv;
|
||||
des(m_ksDES, (unsigned char*)&m_ks); // First initialisation
|
||||
++m_iterations;
|
||||
|
||||
des(m_ksDES, (unsigned char*)&m_ks); // Throw out first iteration & prepare for second
|
||||
++m_iterations;
|
||||
|
||||
generate(64); // Reserved 3 + first 5 of LC (3 left)
|
||||
generate(3 * 8); // Use remaining 3 bytes for LC
|
||||
}
|
||||
|
||||
unsigned long long generate(size_t count) // 1..64
|
||||
{
|
||||
unsigned long long ullCurrent = swap_bytes(m_ks);
|
||||
const int max_len = 64;
|
||||
int pos = m_ks_idx % max_len;
|
||||
|
||||
m_ks_idx += count;
|
||||
|
||||
if ((pos + count) <= max_len) // Up to 64
|
||||
{
|
||||
if ((m_ks_idx % max_len) == 0)
|
||||
{
|
||||
des(m_ksDES, (unsigned char*)&m_ks); // Prepare for next iteration
|
||||
++m_iterations;
|
||||
}
|
||||
|
||||
unsigned long long result = (ullCurrent >> (((max_len - 1) - pos) - (count-1))) & ((count == max_len) ? (unsigned long long)-1 : ((1ULL << count) - 1));
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Over-flow 64-bit boundary (so all of rest of current will be used)
|
||||
|
||||
des(m_ksDES, (unsigned char*)&m_ks); // Compute second part
|
||||
++m_iterations;
|
||||
|
||||
unsigned long long first = ullCurrent << pos; // RHS will be zeros
|
||||
|
||||
ullCurrent = swap_bytes(m_ks);
|
||||
int remainder = count - (max_len - pos);
|
||||
first >>= (((max_len - 1) - remainder) - ((max_len - 1) - pos));
|
||||
unsigned long long next = (ullCurrent >> (((max_len - 1) - 0) - (remainder-1))) & ((1ULL << remainder) - 1);
|
||||
|
||||
return (first | next);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
crypto_module::crypto_module(bool verbose/* = true*/)
|
||||
: d_verbose(verbose)
|
||||
{
|
||||
}
|
||||
|
||||
crypto_algorithm::sptr crypto_module::algorithm(crypto_algorithm::type_id algid)
|
||||
{
|
||||
if ((!d_current_algorithm && (algid == crypto_algorithm::NONE)) || // This line should be commented out if 'null_algorithm' is to be tested
|
||||
(d_current_algorithm && (algid == d_current_algorithm->id())))
|
||||
return d_current_algorithm;
|
||||
|
||||
switch (algid)
|
||||
{
|
||||
case crypto_algorithm::DES_OFB:
|
||||
d_current_algorithm = crypto_algorithm::sptr(new des_ofb());
|
||||
break;
|
||||
//case crypto_algorithm::NONE:
|
||||
// d_current_algorithm = crypto_algorithm::sptr(new null_algorithm());
|
||||
// break;
|
||||
default:
|
||||
d_current_algorithm = crypto_algorithm::sptr();
|
||||
};
|
||||
|
||||
if (d_current_algorithm)
|
||||
{
|
||||
d_current_algorithm->set_logging(logging_enabled());
|
||||
|
||||
if (!d_persistent_key_map.empty())
|
||||
d_current_algorithm->set_key_map(d_persistent_key_map);
|
||||
|
||||
if (!d_persistent_key.empty())
|
||||
d_current_algorithm->set_key(d_persistent_key);
|
||||
}
|
||||
|
||||
return d_current_algorithm;
|
||||
}
|
||||
|
||||
void crypto_module::set_key(const crypto_algorithm::key_type& key)
|
||||
{
|
||||
d_persistent_key = key;
|
||||
|
||||
if (d_current_algorithm)
|
||||
d_current_algorithm->set_key(d_persistent_key);
|
||||
}
|
||||
|
||||
void crypto_module::set_key_map(const crypto_algorithm::key_map_type& keys)
|
||||
{
|
||||
d_persistent_key_map = keys;
|
||||
|
||||
if (d_current_algorithm)
|
||||
d_current_algorithm->set_key_map(d_persistent_key_map);
|
||||
}
|
||||
|
||||
void crypto_module::set_logging(bool on/* = true*/)
|
||||
{
|
||||
d_verbose = on;
|
||||
|
||||
if (d_current_algorithm)
|
||||
d_current_algorithm->set_logging(on);
|
||||
}
|
|
@ -0,0 +1,72 @@
|
|||
#ifndef INCLUDED_CRYPTO_H
|
||||
#define INCLUDED_CRYPTO_H
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
static const int MESSAGE_INDICATOR_LENGTH = 9;
|
||||
|
||||
class CryptoState
|
||||
{
|
||||
public:
|
||||
CryptoState() :
|
||||
kid(0), algid(0), mi(MESSAGE_INDICATOR_LENGTH)
|
||||
{ }
|
||||
public:
|
||||
std::vector<uint8_t> mi;
|
||||
uint16_t kid;
|
||||
uint8_t algid;
|
||||
};
|
||||
|
||||
class crypto_state_provider
|
||||
{
|
||||
public:
|
||||
virtual struct CryptoState crypto_state() const=0;
|
||||
};
|
||||
|
||||
class crypto_algorithm
|
||||
{
|
||||
public:
|
||||
typedef boost::shared_ptr<class crypto_algorithm> sptr;
|
||||
typedef std::vector<uint8_t> key_type;
|
||||
typedef std::map<uint16_t, key_type > key_map_type;
|
||||
typedef uint8_t type_id;
|
||||
enum
|
||||
{
|
||||
NONE = 0x80,
|
||||
DES_OFB = 0x81,
|
||||
};
|
||||
public:
|
||||
virtual const type_id id() const=0;
|
||||
virtual bool set_key(const key_type& key)=0;
|
||||
virtual void set_key_map(const key_map_type& key_map)=0;
|
||||
virtual bool update(const struct CryptoState& state)=0;
|
||||
virtual uint64_t generate(size_t n_bits)=0; // Can request up to 64 bits of key stream at one time
|
||||
virtual void set_logging(bool on)=0;
|
||||
};
|
||||
|
||||
class crypto_module
|
||||
{
|
||||
public:
|
||||
typedef boost::shared_ptr<class crypto_module> sptr;
|
||||
public:
|
||||
crypto_module(bool verbose = false);
|
||||
public:
|
||||
virtual crypto_algorithm::sptr algorithm(crypto_algorithm::type_id algid);
|
||||
virtual void set_key(const crypto_algorithm::key_type& key);
|
||||
virtual void set_key_map(const crypto_algorithm::key_map_type& keys);
|
||||
virtual void set_logging(bool on = true);
|
||||
protected:
|
||||
crypto_algorithm::sptr d_current_algorithm;
|
||||
crypto_algorithm::key_type d_persistent_key;
|
||||
crypto_algorithm::key_map_type d_persistent_key_map;
|
||||
bool d_verbose;
|
||||
public:
|
||||
virtual crypto_algorithm::sptr current_algorithm() const
|
||||
{ return d_current_algorithm; }
|
||||
virtual bool logging_enabled() const
|
||||
{ return d_verbose; }
|
||||
};
|
||||
|
||||
#endif // INCLUDED_CRYPTO_H
|
|
@ -0,0 +1,63 @@
|
|||
#include "crypto_module_du_handler.h"
|
||||
|
||||
#include "abstract_data_unit.h"
|
||||
|
||||
#include <boost/format.hpp>
|
||||
#include <sstream>
|
||||
|
||||
crypto_module_du_handler::crypto_module_du_handler(data_unit_handler_sptr next, crypto_module::sptr crypto_mod)
|
||||
: data_unit_handler(next)
|
||||
, d_crypto_mod(crypto_mod)
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
crypto_module_du_handler::handle(data_unit_sptr du)
|
||||
{
|
||||
if (!d_crypto_mod)
|
||||
{
|
||||
data_unit_handler::handle(du);
|
||||
return;
|
||||
}
|
||||
|
||||
crypto_state_provider* p = dynamic_cast<crypto_state_provider*>(du.get());
|
||||
if (p == NULL)
|
||||
{
|
||||
data_unit_handler::handle(du);
|
||||
return;
|
||||
}
|
||||
|
||||
CryptoState state = p->crypto_state();
|
||||
|
||||
///////////////////////////////////
|
||||
|
||||
if (d_crypto_mod->logging_enabled())
|
||||
{
|
||||
std::string duid_str("?");
|
||||
abstract_data_unit* adu = dynamic_cast<abstract_data_unit*>(du.get());
|
||||
if (adu)
|
||||
duid_str = adu->duid_str();
|
||||
|
||||
std::stringstream ss;
|
||||
for (size_t n = 0; n < state.mi.size(); ++n)
|
||||
ss << (boost::format("%02x") % (int)state.mi[n]);
|
||||
|
||||
fprintf(stderr, "%s:\tAlgID: 0x%02x, KID: 0x%04x, MI: %s\n", duid_str.c_str(), state.algid, state.kid, ss.str().c_str());
|
||||
}
|
||||
|
||||
///////////////////////////////////
|
||||
|
||||
crypto_algorithm::sptr algorithm = d_crypto_mod->algorithm(state.algid);
|
||||
if (!algorithm)
|
||||
{
|
||||
data_unit_handler::handle(du);
|
||||
return;
|
||||
}
|
||||
|
||||
// TODO: Could do key management & selection here with 'state.kid'
|
||||
// Assuming we're only using one key (ignoring 'kid')
|
||||
|
||||
algorithm->update(state);
|
||||
|
||||
data_unit_handler::handle(du);
|
||||
}
|
|
@ -0,0 +1,21 @@
|
|||
#ifndef INCLUDED_CRYPTO_MODULE_DU_HANDLER_H
|
||||
#define INCLUDED_CRYPTO_MODULE_DU_HANDLER_H
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include "data_unit_handler.h"
|
||||
#include "crypto.h"
|
||||
|
||||
class crypto_module_du_handler : public data_unit_handler
|
||||
{
|
||||
public:
|
||||
crypto_module_du_handler(data_unit_handler_sptr next, crypto_module::sptr crypto_mod);
|
||||
public:
|
||||
typedef boost::shared_ptr<class crypto_module_du_handler> sptr;
|
||||
public:
|
||||
virtual void handle(data_unit_sptr du);
|
||||
private:
|
||||
crypto_module::sptr d_crypto_mod;
|
||||
};
|
||||
|
||||
#endif //INCLUDED_CRYPTO_MODULE_HANDLER_H
|
|
@ -32,6 +32,8 @@
|
|||
#include <iosfwd>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "crypto.h"
|
||||
|
||||
typedef std::deque<bool> bit_queue;
|
||||
typedef const std::deque<bool> const_bit_queue;
|
||||
|
||||
|
@ -76,7 +78,7 @@ public:
|
|||
* \precondition is_complete() == true.
|
||||
* \param imbe The imbe_decoder to use to generate the audio.
|
||||
*/
|
||||
virtual void decode_audio(imbe_decoder& imbe) = 0;
|
||||
virtual void decode_audio(imbe_decoder& imbe, crypto_module::sptr crypto_mod) = 0;
|
||||
|
||||
/**
|
||||
* Decode the frame into an octet vector.
|
||||
|
@ -132,6 +134,8 @@ public:
|
|||
*/
|
||||
virtual std::string snapshot() const = 0;
|
||||
|
||||
virtual void set_logging(bool on) = 0;
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
|
|
|
@ -34,6 +34,7 @@
|
|||
#include "offline_imbe_decoder.h"
|
||||
#include "voice_du_handler.h"
|
||||
#include "op25_yank.h"
|
||||
#include "bch.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
@ -41,13 +42,13 @@ namespace gr {
|
|||
namespace op25 {
|
||||
|
||||
decoder_bf::sptr
|
||||
decoder_bf::make()
|
||||
decoder_bf::make(bool idle_silence /*= true*/, bool verbose /*= false*/)
|
||||
{
|
||||
return gnuradio::get_initial_sptr
|
||||
(new decoder_bf_impl());
|
||||
(new decoder_bf_impl(idle_silence, verbose));
|
||||
}
|
||||
|
||||
decoder_bf_impl::decoder_bf_impl() :
|
||||
decoder_bf_impl::decoder_bf_impl(bool idle_silence /*= true*/, bool verbose /*= false*/) :
|
||||
gr::block("decoder_bf",
|
||||
gr::io_signature::make(1, 1, sizeof(uint8_t)),
|
||||
gr::io_signature::make(0, 1, sizeof(float))),
|
||||
|
@ -56,14 +57,25 @@ namespace gr {
|
|||
d_frame_hdr(),
|
||||
d_imbe(imbe_decoder::make()),
|
||||
d_state(SYNCHRONIZING),
|
||||
d_p25cai_du_handler(NULL)
|
||||
d_p25cai_du_handler(NULL),
|
||||
d_idle_silence(idle_silence),
|
||||
d_verbose(false)
|
||||
{
|
||||
set_logging(verbose);
|
||||
|
||||
d_p25cai_du_handler = new p25cai_du_handler(d_data_unit_handler,
|
||||
"224.0.0.1", 23456);
|
||||
d_data_unit_handler = data_unit_handler_sptr(d_p25cai_du_handler);
|
||||
|
||||
d_snapshot_du_handler = new snapshot_du_handler(d_data_unit_handler);
|
||||
d_data_unit_handler = data_unit_handler_sptr(d_snapshot_du_handler);
|
||||
d_data_unit_handler = data_unit_handler_sptr(new voice_du_handler(d_data_unit_handler, d_imbe));
|
||||
|
||||
d_crypto_module = crypto_module::sptr(new crypto_module(verbose));
|
||||
|
||||
d_crypto_module_du_handler = crypto_module_du_handler::sptr(new crypto_module_du_handler(d_data_unit_handler, d_crypto_module));
|
||||
d_data_unit_handler = data_unit_handler_sptr(d_crypto_module_du_handler);
|
||||
|
||||
d_data_unit_handler = data_unit_handler_sptr(new voice_du_handler(d_data_unit_handler, d_imbe, d_crypto_module));
|
||||
}
|
||||
|
||||
decoder_bf_impl::~decoder_bf_impl()
|
||||
|
@ -104,6 +116,7 @@ namespace gr {
|
|||
gr_vector_void_star &output_items)
|
||||
{
|
||||
try {
|
||||
gr::thread::scoped_lock lock(d_mutex);
|
||||
|
||||
// process input
|
||||
const uint8_t *in = reinterpret_cast<const uint8_t*>(input_items[0]);
|
||||
|
@ -121,10 +134,10 @@ namespace gr {
|
|||
copy(samples->begin(), samples->begin() + n, out);
|
||||
samples->erase(samples->begin(), samples->begin() + n);
|
||||
}
|
||||
if(n < noutput_items) {
|
||||
if((d_idle_silence) && (n < noutput_items)) {
|
||||
fill(out + n, out + noutput_items, 0.0);
|
||||
}
|
||||
return noutput_items;
|
||||
return (d_idle_silence ? noutput_items : n);
|
||||
|
||||
} catch(const std::exception& x) {
|
||||
cerr << x.what() << endl;
|
||||
|
@ -177,14 +190,12 @@ namespace gr {
|
|||
};
|
||||
size_t NID_SZ = sizeof(NID) / sizeof(NID[0]);
|
||||
|
||||
itpp::bvec b(63), zeroes(16);
|
||||
itpp::BCH bch(63, 16, 11, "6 3 3 1 1 4 1 3 6 7 2 3 5 4 5 3", true);
|
||||
bit_vector b(NID_SZ);
|
||||
yank(d_frame_hdr, NID, NID_SZ, b, 0);
|
||||
b = bch.decode(b);
|
||||
if(b != zeroes) {
|
||||
b = bch.encode(b);
|
||||
if(bchDec(b) >= 0) {
|
||||
yank_back(b, 0, d_frame_hdr, NID, NID_SZ);
|
||||
d_data_unit = data_unit::make_data_unit(d_frame_hdr);
|
||||
d_data_unit->set_logging(d_verbose);
|
||||
} else {
|
||||
data_unit_sptr null;
|
||||
d_data_unit = null;
|
||||
|
@ -229,5 +240,36 @@ namespace gr {
|
|||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
decoder_bf_impl::set_idle_silence(bool idle_silence/* = true*/)
|
||||
{
|
||||
gr::thread::scoped_lock lock(d_mutex);
|
||||
|
||||
d_idle_silence = idle_silence;
|
||||
}
|
||||
|
||||
void
|
||||
decoder_bf_impl::set_logging(bool verbose/* = true*/)
|
||||
{
|
||||
if (verbose) fprintf(stderr, "[%s<%lu>] verbose logging enabled\n", name().c_str(), unique_id());
|
||||
|
||||
d_verbose = verbose;
|
||||
|
||||
if (d_crypto_module)
|
||||
d_crypto_module->set_logging(verbose);
|
||||
}
|
||||
|
||||
void
|
||||
decoder_bf_impl::set_key(const key_type& key)
|
||||
{
|
||||
d_crypto_module->set_key(key);
|
||||
}
|
||||
|
||||
void
|
||||
decoder_bf_impl::set_key_map(const key_map_type& keys)
|
||||
{
|
||||
d_crypto_module->set_key_map(keys);
|
||||
}
|
||||
} /* namespace op25 */
|
||||
} /* namespace gr */
|
||||
|
|
|
@ -24,11 +24,14 @@
|
|||
#define INCLUDED_OP25_DECODER_BF_IMPL_H
|
||||
|
||||
#include <op25/decoder_bf.h>
|
||||
#include <gnuradio/thread/thread.h>
|
||||
#include "data_unit.h"
|
||||
#include "data_unit_handler.h"
|
||||
#include "imbe_decoder.h"
|
||||
#include "p25cai_du_handler.h"
|
||||
#include "snapshot_du_handler.h"
|
||||
#include "crypto.h"
|
||||
#include "crypto_module_du_handler.h"
|
||||
|
||||
namespace gr {
|
||||
namespace op25 {
|
||||
|
@ -102,8 +105,21 @@ namespace gr {
|
|||
*/
|
||||
class snapshot_du_handler *d_snapshot_du_handler;
|
||||
|
||||
/*
|
||||
* Whether or not to output silence when no audio is synthesised.
|
||||
*/
|
||||
bool d_idle_silence;
|
||||
|
||||
bool d_verbose;
|
||||
|
||||
crypto_module::sptr d_crypto_module;
|
||||
|
||||
crypto_module_du_handler::sptr d_crypto_module_du_handler;
|
||||
|
||||
gr::thread::mutex d_mutex;
|
||||
|
||||
public:
|
||||
decoder_bf_impl();
|
||||
decoder_bf_impl(bool idle_silence = true, bool verbose = false);
|
||||
~decoder_bf_impl();
|
||||
|
||||
// Where all the action really happens
|
||||
|
@ -139,6 +155,14 @@ namespace gr {
|
|||
* message queue.
|
||||
*/
|
||||
void set_msgq(gr::msg_queue::sptr msgq);
|
||||
|
||||
void set_idle_silence(bool idle_silence = true);
|
||||
|
||||
void set_logging(bool verbose = true);
|
||||
|
||||
void set_key(const key_type& key);
|
||||
|
||||
void set_key_map(const key_map_type& keys);
|
||||
};
|
||||
} // namespace op25
|
||||
} // namespace gr
|
||||
|
|
|
@ -34,6 +34,7 @@
|
|||
#include "offline_imbe_decoder.h"
|
||||
#include "voice_du_handler.h"
|
||||
#include "op25_yank.h"
|
||||
#include "bch.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
@ -185,12 +186,9 @@ namespace gr {
|
|||
};
|
||||
size_t NID_SZ = sizeof(NID) / sizeof(NID[0]);
|
||||
|
||||
itpp::bvec b(63), zeroes(16);
|
||||
itpp::BCH bch(63, 16, 11, "6 3 3 1 1 4 1 3 6 7 2 3 5 4 5 3", true);
|
||||
bit_vector b(NID_SZ);
|
||||
yank(d_frame_hdr, NID, NID_SZ, b, 0);
|
||||
b = bch.decode(b);
|
||||
if(b != zeroes) {
|
||||
b = bch.encode(b);
|
||||
if(bchDec(b) >= 0) {
|
||||
yank_back(b, 0, d_frame_hdr, NID, NID_SZ);
|
||||
d_data_unit = data_unit::make_data_unit(d_frame_hdr);
|
||||
} else {
|
||||
|
|
|
@ -0,0 +1,15 @@
|
|||
typedef unsigned long DES_KS[16][2]; /* Single-key DES key schedule */
|
||||
typedef unsigned long DES3_KS[48][2]; /* Triple-DES key schedule */
|
||||
|
||||
/* In deskey.c: */
|
||||
void deskey(DES_KS,unsigned char *,int);
|
||||
void des3key(DES3_KS,unsigned char *,int);
|
||||
|
||||
/* In desport.c, desborl.cas or desgnu.s: */
|
||||
void des(DES_KS,unsigned char *);
|
||||
/* In des3port.c, des3borl.cas or des3gnu.s: */
|
||||
void des3(DES3_KS,unsigned char *);
|
||||
|
||||
extern int Asmversion; /* 1 if we're linked with an asm version, 0 if C */
|
||||
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
/* Portable C code to create DES key schedules from user-provided keys
|
||||
* This doesn't have to be fast unless you're cracking keys or UNIX
|
||||
* passwords
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include "des.h"
|
||||
|
||||
/* Key schedule-related tables from FIPS-46 */
|
||||
|
||||
/* permuted choice table (key) */
|
||||
static unsigned char pc1[] = {
|
||||
57, 49, 41, 33, 25, 17, 9,
|
||||
1, 58, 50, 42, 34, 26, 18,
|
||||
10, 2, 59, 51, 43, 35, 27,
|
||||
19, 11, 3, 60, 52, 44, 36,
|
||||
|
||||
63, 55, 47, 39, 31, 23, 15,
|
||||
7, 62, 54, 46, 38, 30, 22,
|
||||
14, 6, 61, 53, 45, 37, 29,
|
||||
21, 13, 5, 28, 20, 12, 4
|
||||
};
|
||||
|
||||
/* number left rotations of pc1 */
|
||||
static unsigned char totrot[] = {
|
||||
1,2,4,6,8,10,12,14,15,17,19,21,23,25,27,28
|
||||
};
|
||||
|
||||
/* permuted choice key (table) */
|
||||
static unsigned char pc2[] = {
|
||||
14, 17, 11, 24, 1, 5,
|
||||
3, 28, 15, 6, 21, 10,
|
||||
23, 19, 12, 4, 26, 8,
|
||||
16, 7, 27, 20, 13, 2,
|
||||
41, 52, 31, 37, 47, 55,
|
||||
30, 40, 51, 45, 33, 48,
|
||||
44, 49, 39, 56, 34, 53,
|
||||
46, 42, 50, 36, 29, 32
|
||||
};
|
||||
|
||||
/* End of DES-defined tables */
|
||||
|
||||
|
||||
/* bit 0 is left-most in byte */
|
||||
static int bytebit[] = {
|
||||
0200,0100,040,020,010,04,02,01
|
||||
};
|
||||
|
||||
|
||||
/* Generate key schedule for encryption or decryption
|
||||
* depending on the value of "decrypt"
|
||||
*/
|
||||
void
|
||||
deskey(DES_KS k,unsigned char *key,int decrypt)
|
||||
/* Key schedule array */
|
||||
/* 64 bits (will use only 56) */
|
||||
/* 0 = encrypt, 1 = decrypt */
|
||||
{
|
||||
unsigned char pc1m[56]; /* place to modify pc1 into */
|
||||
unsigned char pcr[56]; /* place to rotate pc1 into */
|
||||
register int i,j,l;
|
||||
int m;
|
||||
unsigned char ks[8];
|
||||
|
||||
for (j=0; j<56; j++) { /* convert pc1 to bits of key */
|
||||
l=pc1[j]-1; /* integer bit location */
|
||||
m = l & 07; /* find bit */
|
||||
pc1m[j]=(key[l>>3] & /* find which key byte l is in */
|
||||
bytebit[m]) /* and which bit of that byte */
|
||||
? 1 : 0; /* and store 1-bit result */
|
||||
}
|
||||
for (i=0; i<16; i++) { /* key chunk for each iteration */
|
||||
memset(ks,0,sizeof(ks)); /* Clear key schedule */
|
||||
for (j=0; j<56; j++) /* rotate pc1 the right amount */
|
||||
pcr[j] = pc1m[(l=j+totrot[decrypt? 15-i : i])<(j<28? 28 : 56) ? l: l-28];
|
||||
/* rotate left and right halves independently */
|
||||
for (j=0; j<48; j++){ /* select bits individually */
|
||||
/* check bit that goes to ks[j] */
|
||||
if (pcr[pc2[j]-1]){
|
||||
/* mask it in if it's there */
|
||||
l= j % 6;
|
||||
ks[j/6] |= bytebit[l] >> 2;
|
||||
}
|
||||
}
|
||||
/* Now convert to packed odd/even interleaved form */
|
||||
k[i][0] = ((long)ks[0] << 24)
|
||||
| ((long)ks[2] << 16)
|
||||
| ((long)ks[4] << 8)
|
||||
| ((long)ks[6]);
|
||||
k[i][1] = ((long)ks[1] << 24)
|
||||
| ((long)ks[3] << 16)
|
||||
| ((long)ks[5] << 8)
|
||||
| ((long)ks[7]);
|
||||
if(Asmversion){
|
||||
/* The assembler versions pre-shift each subkey 2 bits
|
||||
* so the Spbox indexes are already computed
|
||||
*/
|
||||
k[i][0] <<= 2;
|
||||
k[i][1] <<= 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Generate key schedule for triple DES in E-D-E (or D-E-D) mode.
|
||||
*
|
||||
* The key argument is taken to be 24 bytes. The first 8 bytes are K1
|
||||
* for the first stage, the second 8 bytes are K2 for the middle stage
|
||||
* and the third 8 bytes are K3 for the last stage
|
||||
*/
|
||||
void
|
||||
des3key(DES3_KS k,unsigned char *key,int decrypt)
|
||||
/* 192 bits (will use only 168) */
|
||||
/* 0 = encrypt, 1 = decrypt */
|
||||
{
|
||||
if(!decrypt){
|
||||
deskey(&k[0],&key[0],0);
|
||||
deskey(&k[16],&key[8],1);
|
||||
deskey(&k[32],&key[16],0);
|
||||
} else {
|
||||
deskey(&k[32],&key[0],1);
|
||||
deskey(&k[16],&key[8],0);
|
||||
deskey(&k[0],&key[16],1);
|
||||
}
|
||||
}
|
|
@ -0,0 +1,235 @@
|
|||
/* Portable C version of des() function */
|
||||
|
||||
#include "des.h"
|
||||
|
||||
/* Tables defined in the Data Encryption Standard documents
|
||||
* Three of these tables, the initial permutation, the final
|
||||
* permutation and the expansion operator, are regular enough that
|
||||
* for speed, we hard-code them. They're here for reference only.
|
||||
* Also, the S and P boxes are used by a separate program, gensp.c,
|
||||
* to build the combined SP box, Spbox[]. They're also here just
|
||||
* for reference.
|
||||
*/
|
||||
#ifdef notdef
|
||||
/* initial permutation IP */
|
||||
static unsigned char ip[] = {
|
||||
58, 50, 42, 34, 26, 18, 10, 2,
|
||||
60, 52, 44, 36, 28, 20, 12, 4,
|
||||
62, 54, 46, 38, 30, 22, 14, 6,
|
||||
64, 56, 48, 40, 32, 24, 16, 8,
|
||||
57, 49, 41, 33, 25, 17, 9, 1,
|
||||
59, 51, 43, 35, 27, 19, 11, 3,
|
||||
61, 53, 45, 37, 29, 21, 13, 5,
|
||||
63, 55, 47, 39, 31, 23, 15, 7
|
||||
};
|
||||
|
||||
/* final permutation IP^-1 */
|
||||
static unsigned char fp[] = {
|
||||
40, 8, 48, 16, 56, 24, 64, 32,
|
||||
39, 7, 47, 15, 55, 23, 63, 31,
|
||||
38, 6, 46, 14, 54, 22, 62, 30,
|
||||
37, 5, 45, 13, 53, 21, 61, 29,
|
||||
36, 4, 44, 12, 52, 20, 60, 28,
|
||||
35, 3, 43, 11, 51, 19, 59, 27,
|
||||
34, 2, 42, 10, 50, 18, 58, 26,
|
||||
33, 1, 41, 9, 49, 17, 57, 25
|
||||
};
|
||||
/* expansion operation matrix */
|
||||
static unsigned char ei[] = {
|
||||
32, 1, 2, 3, 4, 5,
|
||||
4, 5, 6, 7, 8, 9,
|
||||
8, 9, 10, 11, 12, 13,
|
||||
12, 13, 14, 15, 16, 17,
|
||||
16, 17, 18, 19, 20, 21,
|
||||
20, 21, 22, 23, 24, 25,
|
||||
24, 25, 26, 27, 28, 29,
|
||||
28, 29, 30, 31, 32, 1
|
||||
};
|
||||
/* The (in)famous S-boxes */
|
||||
static unsigned char sbox[8][64] = {
|
||||
/* S1 */
|
||||
14, 4, 13, 1, 2, 15, 11, 8, 3, 10, 6, 12, 5, 9, 0, 7,
|
||||
0, 15, 7, 4, 14, 2, 13, 1, 10, 6, 12, 11, 9, 5, 3, 8,
|
||||
4, 1, 14, 8, 13, 6, 2, 11, 15, 12, 9, 7, 3, 10, 5, 0,
|
||||
15, 12, 8, 2, 4, 9, 1, 7, 5, 11, 3, 14, 10, 0, 6, 13,
|
||||
|
||||
/* S2 */
|
||||
15, 1, 8, 14, 6, 11, 3, 4, 9, 7, 2, 13, 12, 0, 5, 10,
|
||||
3, 13, 4, 7, 15, 2, 8, 14, 12, 0, 1, 10, 6, 9, 11, 5,
|
||||
0, 14, 7, 11, 10, 4, 13, 1, 5, 8, 12, 6, 9, 3, 2, 15,
|
||||
13, 8, 10, 1, 3, 15, 4, 2, 11, 6, 7, 12, 0, 5, 14, 9,
|
||||
|
||||
/* S3 */
|
||||
10, 0, 9, 14, 6, 3, 15, 5, 1, 13, 12, 7, 11, 4, 2, 8,
|
||||
13, 7, 0, 9, 3, 4, 6, 10, 2, 8, 5, 14, 12, 11, 15, 1,
|
||||
13, 6, 4, 9, 8, 15, 3, 0, 11, 1, 2, 12, 5, 10, 14, 7,
|
||||
1, 10, 13, 0, 6, 9, 8, 7, 4, 15, 14, 3, 11, 5, 2, 12,
|
||||
|
||||
/* S4 */
|
||||
7, 13, 14, 3, 0, 6, 9, 10, 1, 2, 8, 5, 11, 12, 4, 15,
|
||||
13, 8, 11, 5, 6, 15, 0, 3, 4, 7, 2, 12, 1, 10, 14, 9,
|
||||
10, 6, 9, 0, 12, 11, 7, 13, 15, 1, 3, 14, 5, 2, 8, 4,
|
||||
3, 15, 0, 6, 10, 1, 13, 8, 9, 4, 5, 11, 12, 7, 2, 14,
|
||||
|
||||
/* S5 */
|
||||
2, 12, 4, 1, 7, 10, 11, 6, 8, 5, 3, 15, 13, 0, 14, 9,
|
||||
14, 11, 2, 12, 4, 7, 13, 1, 5, 0, 15, 10, 3, 9, 8, 6,
|
||||
4, 2, 1, 11, 10, 13, 7, 8, 15, 9, 12, 5, 6, 3, 0, 14,
|
||||
11, 8, 12, 7, 1, 14, 2, 13, 6, 15, 0, 9, 10, 4, 5, 3,
|
||||
|
||||
/* S6 */
|
||||
12, 1, 10, 15, 9, 2, 6, 8, 0, 13, 3, 4, 14, 7, 5, 11,
|
||||
10, 15, 4, 2, 7, 12, 9, 5, 6, 1, 13, 14, 0, 11, 3, 8,
|
||||
9, 14, 15, 5, 2, 8, 12, 3, 7, 0, 4, 10, 1, 13, 11, 6,
|
||||
4, 3, 2, 12, 9, 5, 15, 10, 11, 14, 1, 7, 6, 0, 8, 13,
|
||||
|
||||
/* S7 */
|
||||
4, 11, 2, 14, 15, 0, 8, 13, 3, 12, 9, 7, 5, 10, 6, 1,
|
||||
13, 0, 11, 7, 4, 9, 1, 10, 14, 3, 5, 12, 2, 15, 8, 6,
|
||||
1, 4, 11, 13, 12, 3, 7, 14, 10, 15, 6, 8, 0, 5, 9, 2,
|
||||
6, 11, 13, 8, 1, 4, 10, 7, 9, 5, 0, 15, 14, 2, 3, 12,
|
||||
|
||||
/* S8 */
|
||||
13, 2, 8, 4, 6, 15, 11, 1, 10, 9, 3, 14, 5, 0, 12, 7,
|
||||
1, 15, 13, 8, 10, 3, 7, 4, 12, 5, 6, 11, 0, 14, 9, 2,
|
||||
7, 11, 4, 1, 9, 12, 14, 2, 0, 6, 10, 13, 15, 3, 5, 8,
|
||||
2, 1, 14, 7, 4, 10, 8, 13, 15, 12, 9, 0, 3, 5, 6, 11
|
||||
};
|
||||
|
||||
/* 32-bit permutation function P used on the output of the S-boxes */
|
||||
static unsigned char p32i[] = {
|
||||
16, 7, 20, 21,
|
||||
29, 12, 28, 17,
|
||||
1, 15, 23, 26,
|
||||
5, 18, 31, 10,
|
||||
2, 8, 24, 14,
|
||||
32, 27, 3, 9,
|
||||
19, 13, 30, 6,
|
||||
22, 11, 4, 25
|
||||
};
|
||||
#endif
|
||||
|
||||
int Asmversion = 0;
|
||||
|
||||
/* Combined SP lookup table, linked in
|
||||
* For best results, ensure that this is aligned on a 32-bit boundary;
|
||||
* Borland C++ 3.1 doesn't guarantee this!
|
||||
*/
|
||||
extern unsigned long Spbox[8][64]; /* Combined S and P boxes */
|
||||
|
||||
/* Primitive function F.
|
||||
* Input is r, subkey array in keys, output is XORed into l.
|
||||
* Each round consumes eight 6-bit subkeys, one for
|
||||
* each of the 8 S-boxes, 2 longs for each round.
|
||||
* Each long contains four 6-bit subkeys, each taking up a byte.
|
||||
* The first long contains, from high to low end, the subkeys for
|
||||
* S-boxes 1, 3, 5 & 7; the second contains the subkeys for S-boxes
|
||||
* 2, 4, 6 & 8 (using the origin-1 S-box numbering in the standard,
|
||||
* not the origin-0 numbering used elsewhere in this code)
|
||||
* See comments elsewhere about the pre-rotated values of r and Spbox.
|
||||
*/
|
||||
#define F(l,r,key){\
|
||||
work = ((r >> 4) | (r << 28)) ^ key[0];\
|
||||
l ^= Spbox[6][work & 0x3f];\
|
||||
l ^= Spbox[4][(work >> 8) & 0x3f];\
|
||||
l ^= Spbox[2][(work >> 16) & 0x3f];\
|
||||
l ^= Spbox[0][(work >> 24) & 0x3f];\
|
||||
work = r ^ key[1];\
|
||||
l ^= Spbox[7][work & 0x3f];\
|
||||
l ^= Spbox[5][(work >> 8) & 0x3f];\
|
||||
l ^= Spbox[3][(work >> 16) & 0x3f];\
|
||||
l ^= Spbox[1][(work >> 24) & 0x3f];\
|
||||
}
|
||||
/* Encrypt or decrypt a block of data in ECB mode */
|
||||
void
|
||||
des(unsigned long ks[16][2],unsigned char block[8])
|
||||
/* Key schedule */
|
||||
/* Data block */
|
||||
{
|
||||
unsigned long left,right,work;
|
||||
|
||||
/* Read input block and place in left/right in big-endian order */
|
||||
left = ((unsigned long)block[0] << 24)
|
||||
| ((unsigned long)block[1] << 16)
|
||||
| ((unsigned long)block[2] << 8)
|
||||
| (unsigned long)block[3];
|
||||
right = ((unsigned long)block[4] << 24)
|
||||
| ((unsigned long)block[5] << 16)
|
||||
| ((unsigned long)block[6] << 8)
|
||||
| (unsigned long)block[7];
|
||||
|
||||
/* Hoey's clever initial permutation algorithm, from Outerbridge
|
||||
* (see Schneier p 478)
|
||||
*
|
||||
* The convention here is the same as Outerbridge: rotate each
|
||||
* register left by 1 bit, i.e., so that "left" contains permuted
|
||||
* input bits 2, 3, 4, ... 1 and "right" contains 33, 34, 35, ... 32
|
||||
* (using origin-1 numbering as in the FIPS). This allows us to avoid
|
||||
* one of the two rotates that would otherwise be required in each of
|
||||
* the 16 rounds.
|
||||
*/
|
||||
work = ((left >> 4) ^ right) & 0x0f0f0f0f;
|
||||
right ^= work;
|
||||
left ^= work << 4;
|
||||
work = ((left >> 16) ^ right) & 0xffff;
|
||||
right ^= work;
|
||||
left ^= work << 16;
|
||||
work = ((right >> 2) ^ left) & 0x33333333;
|
||||
left ^= work;
|
||||
right ^= (work << 2);
|
||||
work = ((right >> 8) ^ left) & 0xff00ff;
|
||||
left ^= work;
|
||||
right ^= (work << 8);
|
||||
right = (right << 1) | (right >> 31);
|
||||
work = (left ^ right) & 0xaaaaaaaa;
|
||||
left ^= work;
|
||||
right ^= work;
|
||||
left = (left << 1) | (left >> 31);
|
||||
|
||||
/* Now do the 16 rounds */
|
||||
F(left,right,ks[0]);
|
||||
F(right,left,ks[1]);
|
||||
F(left,right,ks[2]);
|
||||
F(right,left,ks[3]);
|
||||
F(left,right,ks[4]);
|
||||
F(right,left,ks[5]);
|
||||
F(left,right,ks[6]);
|
||||
F(right,left,ks[7]);
|
||||
F(left,right,ks[8]);
|
||||
F(right,left,ks[9]);
|
||||
F(left,right,ks[10]);
|
||||
F(right,left,ks[11]);
|
||||
F(left,right,ks[12]);
|
||||
F(right,left,ks[13]);
|
||||
F(left,right,ks[14]);
|
||||
F(right,left,ks[15]);
|
||||
|
||||
/* Inverse permutation, also from Hoey via Outerbridge and Schneier */
|
||||
right = (right << 31) | (right >> 1);
|
||||
work = (left ^ right) & 0xaaaaaaaa;
|
||||
left ^= work;
|
||||
right ^= work;
|
||||
left = (left >> 1) | (left << 31);
|
||||
work = ((left >> 8) ^ right) & 0xff00ff;
|
||||
right ^= work;
|
||||
left ^= work << 8;
|
||||
work = ((left >> 2) ^ right) & 0x33333333;
|
||||
right ^= work;
|
||||
left ^= work << 2;
|
||||
work = ((right >> 16) ^ left) & 0xffff;
|
||||
left ^= work;
|
||||
right ^= work << 16;
|
||||
work = ((right >> 4) ^ left) & 0x0f0f0f0f;
|
||||
left ^= work;
|
||||
right ^= work << 4;
|
||||
|
||||
/* Put the block back into the user's buffer with final swap */
|
||||
block[0] = right >> 24;
|
||||
block[1] = right >> 16;
|
||||
block[2] = right >> 8;
|
||||
block[3] = right;
|
||||
block[4] = left >> 24;
|
||||
block[5] = left >> 16;
|
||||
block[6] = left >> 8;
|
||||
block[7] = left;
|
||||
}
|
|
@ -0,0 +1,130 @@
|
|||
unsigned long Spbox[8][64] = {
|
||||
0x01010400,0x00000000,0x00010000,0x01010404,
|
||||
0x01010004,0x00010404,0x00000004,0x00010000,
|
||||
0x00000400,0x01010400,0x01010404,0x00000400,
|
||||
0x01000404,0x01010004,0x01000000,0x00000004,
|
||||
0x00000404,0x01000400,0x01000400,0x00010400,
|
||||
0x00010400,0x01010000,0x01010000,0x01000404,
|
||||
0x00010004,0x01000004,0x01000004,0x00010004,
|
||||
0x00000000,0x00000404,0x00010404,0x01000000,
|
||||
0x00010000,0x01010404,0x00000004,0x01010000,
|
||||
0x01010400,0x01000000,0x01000000,0x00000400,
|
||||
0x01010004,0x00010000,0x00010400,0x01000004,
|
||||
0x00000400,0x00000004,0x01000404,0x00010404,
|
||||
0x01010404,0x00010004,0x01010000,0x01000404,
|
||||
0x01000004,0x00000404,0x00010404,0x01010400,
|
||||
0x00000404,0x01000400,0x01000400,0x00000000,
|
||||
0x00010004,0x00010400,0x00000000,0x01010004,
|
||||
0x80108020,0x80008000,0x00008000,0x00108020,
|
||||
0x00100000,0x00000020,0x80100020,0x80008020,
|
||||
0x80000020,0x80108020,0x80108000,0x80000000,
|
||||
0x80008000,0x00100000,0x00000020,0x80100020,
|
||||
0x00108000,0x00100020,0x80008020,0x00000000,
|
||||
0x80000000,0x00008000,0x00108020,0x80100000,
|
||||
0x00100020,0x80000020,0x00000000,0x00108000,
|
||||
0x00008020,0x80108000,0x80100000,0x00008020,
|
||||
0x00000000,0x00108020,0x80100020,0x00100000,
|
||||
0x80008020,0x80100000,0x80108000,0x00008000,
|
||||
0x80100000,0x80008000,0x00000020,0x80108020,
|
||||
0x00108020,0x00000020,0x00008000,0x80000000,
|
||||
0x00008020,0x80108000,0x00100000,0x80000020,
|
||||
0x00100020,0x80008020,0x80000020,0x00100020,
|
||||
0x00108000,0x00000000,0x80008000,0x00008020,
|
||||
0x80000000,0x80100020,0x80108020,0x00108000,
|
||||
0x00000208,0x08020200,0x00000000,0x08020008,
|
||||
0x08000200,0x00000000,0x00020208,0x08000200,
|
||||
0x00020008,0x08000008,0x08000008,0x00020000,
|
||||
0x08020208,0x00020008,0x08020000,0x00000208,
|
||||
0x08000000,0x00000008,0x08020200,0x00000200,
|
||||
0x00020200,0x08020000,0x08020008,0x00020208,
|
||||
0x08000208,0x00020200,0x00020000,0x08000208,
|
||||
0x00000008,0x08020208,0x00000200,0x08000000,
|
||||
0x08020200,0x08000000,0x00020008,0x00000208,
|
||||
0x00020000,0x08020200,0x08000200,0x00000000,
|
||||
0x00000200,0x00020008,0x08020208,0x08000200,
|
||||
0x08000008,0x00000200,0x00000000,0x08020008,
|
||||
0x08000208,0x00020000,0x08000000,0x08020208,
|
||||
0x00000008,0x00020208,0x00020200,0x08000008,
|
||||
0x08020000,0x08000208,0x00000208,0x08020000,
|
||||
0x00020208,0x00000008,0x08020008,0x00020200,
|
||||
0x100802001,0x100002081,0x100002081,0x00000080,
|
||||
0x00802080,0x100800081,0x100800001,0x100002001,
|
||||
0x00000000,0x00802000,0x00802000,0x100802081,
|
||||
0x100000081,0x00000000,0x00800080,0x100800001,
|
||||
0x100000001,0x00002000,0x00800000,0x100802001,
|
||||
0x00000080,0x00800000,0x100002001,0x00002080,
|
||||
0x100800081,0x100000001,0x00002080,0x00800080,
|
||||
0x00002000,0x00802080,0x100802081,0x100000081,
|
||||
0x00800080,0x100800001,0x00802000,0x100802081,
|
||||
0x100000081,0x00000000,0x00000000,0x00802000,
|
||||
0x00002080,0x00800080,0x100800081,0x100000001,
|
||||
0x100802001,0x100002081,0x100002081,0x00000080,
|
||||
0x100802081,0x100000081,0x100000001,0x00002000,
|
||||
0x100800001,0x100002001,0x00802080,0x100800081,
|
||||
0x100002001,0x00002080,0x00800000,0x100802001,
|
||||
0x00000080,0x00800000,0x00002000,0x00802080,
|
||||
0x00000100,0x02080100,0x02080000,0x42000100,
|
||||
0x00080000,0x00000100,0x40000000,0x02080000,
|
||||
0x40080100,0x00080000,0x02000100,0x40080100,
|
||||
0x42000100,0x42080000,0x00080100,0x40000000,
|
||||
0x02000000,0x40080000,0x40080000,0x00000000,
|
||||
0x40000100,0x42080100,0x42080100,0x02000100,
|
||||
0x42080000,0x40000100,0x00000000,0x42000000,
|
||||
0x02080100,0x02000000,0x42000000,0x00080100,
|
||||
0x00080000,0x42000100,0x00000100,0x02000000,
|
||||
0x40000000,0x02080000,0x42000100,0x40080100,
|
||||
0x02000100,0x40000000,0x42080000,0x02080100,
|
||||
0x40080100,0x00000100,0x02000000,0x42080000,
|
||||
0x42080100,0x00080100,0x42000000,0x42080100,
|
||||
0x02080000,0x00000000,0x40080000,0x42000000,
|
||||
0x00080100,0x02000100,0x40000100,0x00080000,
|
||||
0x00000000,0x40080000,0x02080100,0x40000100,
|
||||
0x20000010,0x20400000,0x00004000,0x20404010,
|
||||
0x20400000,0x00000010,0x20404010,0x00400000,
|
||||
0x20004000,0x00404010,0x00400000,0x20000010,
|
||||
0x00400010,0x20004000,0x20000000,0x00004010,
|
||||
0x00000000,0x00400010,0x20004010,0x00004000,
|
||||
0x00404000,0x20004010,0x00000010,0x20400010,
|
||||
0x20400010,0x00000000,0x00404010,0x20404000,
|
||||
0x00004010,0x00404000,0x20404000,0x20000000,
|
||||
0x20004000,0x00000010,0x20400010,0x00404000,
|
||||
0x20404010,0x00400000,0x00004010,0x20000010,
|
||||
0x00400000,0x20004000,0x20000000,0x00004010,
|
||||
0x20000010,0x20404010,0x00404000,0x20400000,
|
||||
0x00404010,0x20404000,0x00000000,0x20400010,
|
||||
0x00000010,0x00004000,0x20400000,0x00404010,
|
||||
0x00004000,0x00400010,0x20004010,0x00000000,
|
||||
0x20404000,0x20000000,0x00400010,0x20004010,
|
||||
0x00200000,0x04200002,0x04000802,0x00000000,
|
||||
0x00000800,0x04000802,0x00200802,0x04200800,
|
||||
0x04200802,0x00200000,0x00000000,0x04000002,
|
||||
0x00000002,0x04000000,0x04200002,0x00000802,
|
||||
0x04000800,0x00200802,0x00200002,0x04000800,
|
||||
0x04000002,0x04200000,0x04200800,0x00200002,
|
||||
0x04200000,0x00000800,0x00000802,0x04200802,
|
||||
0x00200800,0x00000002,0x04000000,0x00200800,
|
||||
0x04000000,0x00200800,0x00200000,0x04000802,
|
||||
0x04000802,0x04200002,0x04200002,0x00000002,
|
||||
0x00200002,0x04000000,0x04000800,0x00200000,
|
||||
0x04200800,0x00000802,0x00200802,0x04200800,
|
||||
0x00000802,0x04000002,0x04200802,0x04200000,
|
||||
0x00200800,0x00000000,0x00000002,0x04200802,
|
||||
0x00000000,0x00200802,0x04200000,0x00000800,
|
||||
0x04000002,0x04000800,0x00000800,0x00200002,
|
||||
0x10001040,0x00001000,0x00040000,0x10041040,
|
||||
0x10000000,0x10001040,0x00000040,0x10000000,
|
||||
0x00040040,0x10040000,0x10041040,0x00041000,
|
||||
0x10041000,0x00041040,0x00001000,0x00000040,
|
||||
0x10040000,0x10000040,0x10001000,0x00001040,
|
||||
0x00041000,0x00040040,0x10040040,0x10041000,
|
||||
0x00001040,0x00000000,0x00000000,0x10040040,
|
||||
0x10000040,0x10001000,0x00041040,0x00040000,
|
||||
0x00041040,0x00040000,0x10041000,0x00001000,
|
||||
0x00000040,0x10040040,0x00001000,0x00041040,
|
||||
0x10001000,0x00000040,0x10000040,0x10040000,
|
||||
0x10040040,0x10000000,0x00040000,0x10001040,
|
||||
0x00000000,0x10041040,0x00040040,0x10000040,
|
||||
0x10040000,0x10001000,0x10001040,0x00000000,
|
||||
0x10041040,0x00041000,0x00041000,0x00001040,
|
||||
0x00001040,0x00040040,0x10000000,0x10041000,
|
||||
};
|
|
@ -28,6 +28,7 @@
|
|||
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <boost/format.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
@ -65,6 +66,8 @@ hdu::do_correct_errors(bit_vector& frame)
|
|||
{
|
||||
apply_golay_correction(frame);
|
||||
apply_rs_correction(frame);
|
||||
|
||||
if (logging_enabled()) fprintf(stderr, "\n");
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -136,33 +139,58 @@ hdu::frame_size_max() const
|
|||
return 792;
|
||||
}
|
||||
|
||||
string
|
||||
hdu::algid_str() const
|
||||
uint8_t
|
||||
hdu::algid() const
|
||||
{
|
||||
const size_t ALGID_BITS[] = {
|
||||
356, 357, 360, 361, 374, 375, 376, 377
|
||||
};
|
||||
const size_t ALGID_BITS_SZ = sizeof(ALGID_BITS) / sizeof(ALGID_BITS[0]);
|
||||
uint8_t algid = extract(frame_body(), ALGID_BITS, ALGID_BITS_SZ);
|
||||
return lookup(algid, ALGIDS, ALGIDS_SZ);
|
||||
return extract(frame_body(), ALGID_BITS, ALGID_BITS_SZ);
|
||||
}
|
||||
|
||||
string
|
||||
hdu::kid_str() const
|
||||
hdu::algid_str() const
|
||||
{
|
||||
uint8_t _algid = algid();
|
||||
return lookup(_algid, ALGIDS, ALGIDS_SZ);
|
||||
}
|
||||
|
||||
uint16_t
|
||||
hdu::kid() const
|
||||
{
|
||||
const size_t KID_BITS[] = {
|
||||
378, 379, 392, 393, 394, 395, 396, 397,
|
||||
410, 411, 412, 413, 414, 415, 428, 429
|
||||
};
|
||||
const size_t KID_BITS_SZ = sizeof(KID_BITS) / sizeof(KID_BITS[0]);
|
||||
uint16_t kid = extract(frame_body(), KID_BITS, KID_BITS_SZ);
|
||||
return extract(frame_body(), KID_BITS, KID_BITS_SZ);
|
||||
}
|
||||
|
||||
string
|
||||
hdu::kid_str() const
|
||||
{
|
||||
uint16_t _kid = kid();
|
||||
ostringstream os;
|
||||
os << hex << showbase << setfill('0') << setw(4) << kid;
|
||||
os << hex << showbase << setfill('0') << setw(4) << _kid;
|
||||
return os.str();
|
||||
}
|
||||
|
||||
std::string
|
||||
hdu::mi_str() const
|
||||
{
|
||||
std::vector<uint8_t> _mi(mi());
|
||||
ostringstream os;
|
||||
os << "0x";
|
||||
for(size_t i = 0; i < _mi.size(); ++i) {
|
||||
uint16_t octet = _mi[i];
|
||||
os << hex << setfill('0') << setw(2) << octet;
|
||||
}
|
||||
return os.str();
|
||||
}
|
||||
|
||||
std::vector<uint8_t>
|
||||
hdu::mi() const
|
||||
{
|
||||
const size_t MI_BITS[] = {
|
||||
114, 115, 116, 117, 118, 119, 132, 133,
|
||||
|
@ -177,15 +205,9 @@ hdu::mi_str() const
|
|||
};
|
||||
const size_t MI_BITS_SZ = sizeof(MI_BITS) / sizeof(MI_BITS[0]);
|
||||
|
||||
uint8_t mi[9];
|
||||
extract(frame_body(), MI_BITS, MI_BITS_SZ, mi);
|
||||
ostringstream os;
|
||||
os << "0x";
|
||||
for(size_t i = 0; i < (sizeof(mi) / sizeof(mi[0])); ++i) {
|
||||
uint16_t octet = mi[i];
|
||||
os << hex << setfill('0') << setw(2) << octet;
|
||||
}
|
||||
return os.str();
|
||||
std::vector<uint8_t> _mi(((MI_BITS_SZ + 7) / 8));
|
||||
extract(frame_body(), MI_BITS, MI_BITS_SZ, &_mi[0]);
|
||||
return _mi;
|
||||
}
|
||||
|
||||
string
|
||||
|
@ -219,7 +241,21 @@ hdu::tgid_str() const
|
|||
};
|
||||
const size_t TGID_BITS_SZ = sizeof(TGID_BITS) / sizeof(TGID_BITS[0]);
|
||||
const uint16_t tgid = extract(frame_body(), TGID_BITS, TGID_BITS_SZ);
|
||||
ostringstream os;
|
||||
os << hex << showbase << setfill('0') << setw(4) << tgid;
|
||||
return os.str();
|
||||
// Zero fill isn't working properly in original implementation
|
||||
//ostringstream os;
|
||||
//os << hex << showbase << setfill('0') << setw(4) << tgid;
|
||||
//return os.str();
|
||||
return (boost::format("0x%04x") % tgid).str();
|
||||
}
|
||||
|
||||
struct CryptoState
|
||||
hdu::crypto_state() const
|
||||
{
|
||||
struct CryptoState state;
|
||||
|
||||
state.mi = mi();
|
||||
state.kid = kid();
|
||||
state.algid = algid();
|
||||
|
||||
return state;
|
||||
}
|
||||
|
|
|
@ -25,11 +25,12 @@
|
|||
#define INCLUDED_HDU_H
|
||||
|
||||
#include "abstract_data_unit.h"
|
||||
#include "crypto.h"
|
||||
|
||||
/**
|
||||
* P25 header data unit (HDU).
|
||||
*/
|
||||
class hdu : public abstract_data_unit
|
||||
class hdu : public abstract_data_unit, public crypto_state_provider
|
||||
{
|
||||
|
||||
public:
|
||||
|
@ -96,7 +97,9 @@ protected:
|
|||
*/
|
||||
virtual uint16_t frame_size_max() const;
|
||||
|
||||
private:
|
||||
public:
|
||||
|
||||
uint8_t algid() const;
|
||||
|
||||
/**
|
||||
* Return a string describing the encryption algorithm ID (ALGID).
|
||||
|
@ -105,6 +108,8 @@ private:
|
|||
*/
|
||||
std::string algid_str() const;
|
||||
|
||||
virtual uint16_t kid() const;
|
||||
|
||||
/**
|
||||
* Returns a string describing the key id (KID).
|
||||
*
|
||||
|
@ -119,6 +124,8 @@ private:
|
|||
*/
|
||||
virtual std::string mfid_str() const;
|
||||
|
||||
virtual std::vector<uint8_t> mi() const;
|
||||
|
||||
/**
|
||||
* Returns a string describing the message indicator (MI).
|
||||
*
|
||||
|
@ -139,6 +146,10 @@ private:
|
|||
* \return A string identifying the TGID.
|
||||
*/
|
||||
virtual std::string tgid_str() const;
|
||||
|
||||
public:
|
||||
|
||||
struct CryptoState crypto_state() const;
|
||||
};
|
||||
|
||||
#endif /* INCLUDED_HDU_H */
|
||||
|
|
|
@ -0,0 +1,100 @@
|
|||
#include "ldu.h"
|
||||
|
||||
#include <itpp/base/vec.h>
|
||||
#include <itpp/base/mat.h>
|
||||
#include <itpp/base/binary.h>
|
||||
#include <itpp/base/converters.h>
|
||||
|
||||
const static itpp::Mat<int> ham_10_6_3_6("1 1 1 0 0 1 1 0 0 0; 1 1 0 1 0 1 0 1 0 0; 1 0 1 1 1 0 0 0 1 0; 0 1 1 1 1 0 0 0 0 1");
|
||||
|
||||
typedef std::vector<itpp::Vec<int> > VecArray;
|
||||
|
||||
ldu::ldu(const_bit_queue& frame_body) :
|
||||
voice_data_unit(frame_body),
|
||||
m_hamming_error_count(0)
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
ldu::do_correct_errors(bit_vector& frame_body)
|
||||
{
|
||||
voice_data_unit::do_correct_errors(frame_body);
|
||||
}
|
||||
|
||||
bool
|
||||
ldu::process_meta_data(bit_vector& frame_body)
|
||||
{
|
||||
m_hamming_error_count = 0;
|
||||
|
||||
//std::vector<uint8_t> lc(30);
|
||||
//std::vector<uint16_t> ham(24);
|
||||
int lc_bit_idx = 0;
|
||||
VecArray arrayVec;
|
||||
itpp::Vec<int> vecRaw(10); // First 6 bits contain data
|
||||
|
||||
for (int i = 400; i < 1360; i += 184)
|
||||
{
|
||||
for (int j = 0; j < 40; j++)
|
||||
{
|
||||
int x = (i + j) + (((i + j) / 70) * 2); // Adjust bit index for status
|
||||
unsigned char ch = frame_body[x];
|
||||
|
||||
//lc[lc_bit_idx / 8] |= (ch << (7 - (lc_bit_idx % 8)));
|
||||
//ham[lc_bit_idx / 10] = ((ham[lc_bit_idx / 10]) << 1) | ch;
|
||||
vecRaw(lc_bit_idx % 10) = ch;
|
||||
|
||||
++lc_bit_idx;
|
||||
|
||||
if ((lc_bit_idx % 10) == 0)
|
||||
arrayVec.push_back(vecRaw);
|
||||
}
|
||||
}
|
||||
|
||||
if (lc_bit_idx != 240) // Not enough bits
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (arrayVec.size() != 24) // Not enough vectors
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
itpp::Vec<int> vecZero(4);
|
||||
vecZero.zeros();
|
||||
|
||||
m_raw_meta_data.clear();
|
||||
|
||||
for (int i = 0; i < arrayVec.size(); ++i)
|
||||
{
|
||||
itpp::Vec<int>& vec = arrayVec[i];
|
||||
itpp::bvec vB(itpp::to_bvec(vec));
|
||||
|
||||
itpp::Vec<int> vS = ham_10_6_3_6 * vec;
|
||||
for (int i = 0; i < vS.length(); ++i)
|
||||
vS[i] = vS[i] % 2;
|
||||
itpp::bvec vb(to_bvec(vS));
|
||||
if (itpp::bin2dec(vb) != 0)
|
||||
{
|
||||
++m_hamming_error_count;
|
||||
}
|
||||
|
||||
m_raw_meta_data = concat(m_raw_meta_data, vB.mid(0, 6)); // Includes RS for last 72 bits
|
||||
}
|
||||
|
||||
if (logging_enabled()) fprintf(stderr, "%s: %lu hamming errors, %s\n", duid_str().c_str(), m_hamming_error_count, (meta_data_valid() ? "valid" : "invalid"));
|
||||
|
||||
return meta_data_valid();
|
||||
}
|
||||
|
||||
const itpp::bvec&
|
||||
ldu::raw_meta_data() const
|
||||
{
|
||||
return m_raw_meta_data;
|
||||
}
|
||||
|
||||
bool
|
||||
ldu::meta_data_valid() const
|
||||
{
|
||||
return (m_raw_meta_data.length() == 144); // Not enough bits after Hamming(10,6,3)
|
||||
}
|
|
@ -0,0 +1,28 @@
|
|||
#ifndef INCLUDED_LDU_H
|
||||
#define INCLUDED_LDU_H
|
||||
|
||||
#include "voice_data_unit.h"
|
||||
|
||||
class ldu : public voice_data_unit
|
||||
{
|
||||
private:
|
||||
|
||||
size_t m_hamming_error_count;
|
||||
itpp::bvec m_raw_meta_data;
|
||||
|
||||
protected:
|
||||
|
||||
virtual void do_correct_errors(bit_vector& frame_body);
|
||||
|
||||
virtual bool process_meta_data(bit_vector& frame_body);
|
||||
|
||||
virtual const itpp::bvec& raw_meta_data() const;
|
||||
|
||||
public:
|
||||
|
||||
ldu(const_bit_queue& frame_body);
|
||||
|
||||
virtual bool meta_data_valid() const;
|
||||
};
|
||||
|
||||
#endif // INCLUDED_LDU_H
|
|
@ -23,10 +23,19 @@
|
|||
|
||||
#include "ldu1.h"
|
||||
|
||||
#include <itpp/base/vec.h>
|
||||
#include <itpp/base/converters.h>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
#include <iostream>
|
||||
|
||||
#include "pickle.h"
|
||||
#include "value_string.h"
|
||||
|
||||
using std::string;
|
||||
|
||||
ldu1::ldu1(const_bit_queue& frame_body) :
|
||||
voice_data_unit(frame_body)
|
||||
ldu(frame_body)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -34,6 +43,64 @@ ldu1::~ldu1()
|
|||
{
|
||||
}
|
||||
|
||||
void ldu1::do_correct_errors(bit_vector& frame_body)
|
||||
{
|
||||
ldu::do_correct_errors(frame_body);
|
||||
|
||||
if (!process_meta_data(frame_body))
|
||||
return;
|
||||
|
||||
const itpp::bvec& data = raw_meta_data();
|
||||
|
||||
std::stringstream ss;
|
||||
|
||||
m_meta_data.m.lcf = bin2dec(data.mid(0, 8));
|
||||
m_meta_data.m.mfid = bin2dec(data.mid(8, 8));
|
||||
ss << (boost::format("%s: LCF: 0x%02x, MFID: 0x%02x") % duid_str() % m_meta_data.m.lcf % m_meta_data.m.mfid);
|
||||
if (m_meta_data.m.lcf == 0x00)
|
||||
{
|
||||
m_meta_data.m0.emergency = data[16];
|
||||
m_meta_data.m0.reserved = bin2dec(data.mid(17, 15));
|
||||
m_meta_data.m0.tgid = bin2dec(data.mid(32, 16));
|
||||
m_meta_data.m0.source = bin2dec(data.mid(48, 24));
|
||||
ss << (boost::format(", Emergency: 0x%02x, Reserved: 0x%04x, TGID: 0x%04x, Source: 0x%06x") % m_meta_data.m0.emergency % m_meta_data.m0.reserved % m_meta_data.m0.tgid % m_meta_data.m0.source);
|
||||
}
|
||||
else if (m_meta_data.m.lcf == 0x03)
|
||||
{
|
||||
m_meta_data.m3.reserved = bin2dec(data.mid(16, 8));
|
||||
m_meta_data.m3.destination = bin2dec(data.mid(24, 24));
|
||||
m_meta_data.m3.source = bin2dec(data.mid(48, 24));
|
||||
ss << (boost::format(", Reserved: 0x%02x, Destination: 0x%06x, Source: 0x%06x") % m_meta_data.m3.reserved % m_meta_data.m3.destination % m_meta_data.m3.source);
|
||||
}
|
||||
else
|
||||
{
|
||||
ss << " (unknown LCF)";
|
||||
}
|
||||
|
||||
if (logging_enabled()) std::cerr << ss.str() << std::endl;
|
||||
}
|
||||
|
||||
std::string
|
||||
ldu1::snapshot() const
|
||||
{
|
||||
pickle p;
|
||||
p.add("duid", duid_str());
|
||||
p.add("mfid", lookup(m_meta_data.m.mfid, MFIDS, MFIDS_SZ));
|
||||
if ((m_meta_data.m.lcf == 0x00) || (m_meta_data.m.lcf == 0x03))
|
||||
p.add("source", (boost::format("0x%06x") % m_meta_data.m0.source).str());
|
||||
if (m_meta_data.m.lcf == 0x00)
|
||||
p.add("tgid", (boost::format("0x%04x") % m_meta_data.m0.tgid).str());
|
||||
if (m_meta_data.m.lcf == 0x03)
|
||||
p.add("dest", (boost::format("0x%06x") % m_meta_data.m3.destination).str());
|
||||
return p.to_string();
|
||||
}
|
||||
|
||||
ldu1::combined_meta_data
|
||||
ldu1::meta_data() const
|
||||
{
|
||||
return m_meta_data;
|
||||
}
|
||||
|
||||
string
|
||||
ldu1::duid_str() const
|
||||
{
|
||||
|
|
|
@ -24,13 +24,45 @@
|
|||
#ifndef INCLUDED_LDU1_H
|
||||
#define INCLUDED_LDU1_H
|
||||
|
||||
#include "voice_data_unit.h"
|
||||
#include "ldu.h"
|
||||
|
||||
/**
|
||||
* P25 Logical Data Unit 1.
|
||||
*/
|
||||
class ldu1 : public voice_data_unit
|
||||
class ldu1 : public ldu
|
||||
{
|
||||
protected:
|
||||
|
||||
void do_correct_errors(bit_vector& frame_body);
|
||||
|
||||
public:
|
||||
|
||||
struct base_meta_data
|
||||
{
|
||||
unsigned char lcf;
|
||||
unsigned char mfid;
|
||||
unsigned int source;
|
||||
unsigned short reserved;
|
||||
};
|
||||
|
||||
struct meta_data_0 : public base_meta_data
|
||||
{
|
||||
bool emergency;
|
||||
unsigned short tgid;
|
||||
};
|
||||
|
||||
struct meta_data_3 : public base_meta_data
|
||||
{
|
||||
unsigned int destination;
|
||||
};
|
||||
|
||||
union combined_meta_data
|
||||
{
|
||||
base_meta_data m;
|
||||
meta_data_0 m0;
|
||||
meta_data_3 m3;
|
||||
} m_meta_data;
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
|
@ -50,6 +82,10 @@ public:
|
|||
*/
|
||||
std::string duid_str() const;
|
||||
|
||||
virtual std::string snapshot() const;
|
||||
|
||||
combined_meta_data meta_data() const;
|
||||
|
||||
};
|
||||
|
||||
#endif /* INCLUDED_LDU1_H */
|
||||
|
|
|
@ -23,10 +23,18 @@
|
|||
|
||||
#include "ldu2.h"
|
||||
|
||||
#include <itpp/base/vec.h>
|
||||
#include <itpp/base/converters.h>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
|
||||
#include "pickle.h"
|
||||
#include "value_string.h"
|
||||
|
||||
using std::string;
|
||||
|
||||
ldu2::ldu2(const_bit_queue& frame_body) :
|
||||
voice_data_unit(frame_body)
|
||||
ldu(frame_body)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -39,3 +47,43 @@ ldu2::duid_str() const
|
|||
{
|
||||
return string("LDU2");
|
||||
}
|
||||
|
||||
std::string
|
||||
ldu2::snapshot() const
|
||||
{
|
||||
pickle p;
|
||||
p.add("duid", duid_str());
|
||||
std::stringstream ss;
|
||||
ss << "0x";
|
||||
for (size_t n = 0; n < m_crypto_state.mi.size(); ++n)
|
||||
ss << (boost::format("%02x") % (int)m_crypto_state.mi[n]);
|
||||
p.add("mi", ss.str());
|
||||
p.add("algid", lookup(m_crypto_state.algid, ALGIDS, ALGIDS_SZ));
|
||||
p.add("kid", (boost::format("0x%04x") % m_crypto_state.kid).str());
|
||||
return p.to_string();
|
||||
}
|
||||
|
||||
void
|
||||
ldu2::do_correct_errors(bit_vector& frame_body)
|
||||
{
|
||||
ldu::do_correct_errors(frame_body);
|
||||
|
||||
if (!process_meta_data(frame_body))
|
||||
return;
|
||||
|
||||
const itpp::bvec& data = raw_meta_data();
|
||||
|
||||
for (int i = 0; i < 72; i += 8)
|
||||
{
|
||||
m_crypto_state.mi[i/8] = bin2dec(data.mid(i, 8));
|
||||
}
|
||||
|
||||
m_crypto_state.algid = bin2dec(data.mid(72, 8));
|
||||
m_crypto_state.kid = bin2dec(data.mid(80, 16));
|
||||
}
|
||||
|
||||
struct CryptoState
|
||||
ldu2::crypto_state() const
|
||||
{
|
||||
return m_crypto_state;
|
||||
}
|
||||
|
|
|
@ -24,13 +24,22 @@
|
|||
#ifndef INCLUDED_LDU2_H
|
||||
#define INCLUDED_LDU2_H
|
||||
|
||||
#include "voice_data_unit.h"
|
||||
#include "ldu.h"
|
||||
#include "crypto.h"
|
||||
|
||||
/**
|
||||
* P25 Logical Data Unit 2.
|
||||
*/
|
||||
class ldu2 : public voice_data_unit
|
||||
class ldu2 : public ldu, public crypto_state_provider
|
||||
{
|
||||
private:
|
||||
|
||||
struct CryptoState m_crypto_state;
|
||||
|
||||
protected:
|
||||
|
||||
void do_correct_errors(bit_vector& frame_body);
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
|
@ -49,6 +58,10 @@ public:
|
|||
* Returns a string describing the Data Unit ID (DUID).
|
||||
*/
|
||||
std::string duid_str() const;
|
||||
|
||||
virtual std::string snapshot() const;
|
||||
|
||||
struct CryptoState crypto_state() const;
|
||||
};
|
||||
|
||||
#endif /* INCLUDED_LDU2_H */
|
||||
|
|
|
@ -1,113 +0,0 @@
|
|||
/* -*- C++ -*- */
|
||||
|
||||
%feature("autodoc", "1");
|
||||
|
||||
%{
|
||||
#include <stddef.h>
|
||||
%}
|
||||
|
||||
%include "exception.i"
|
||||
%import "gnuradio.i"
|
||||
|
||||
%{
|
||||
#include "gnuradio/swig/gnuradio_swig_bug_workaround.h"
|
||||
#include "op25_fsk4_demod_ff.h"
|
||||
#include "op25_fsk4_slicer_fb.h"
|
||||
#include "op25_decoder_bf.h"
|
||||
#include "op25_pcap_source_b.h"
|
||||
%}
|
||||
|
||||
// ----------------------------------------------------------------
|
||||
|
||||
/*
|
||||
* This does some behind-the-scenes magic so we can
|
||||
* access fsk4_square_ff from python as fsk4.square_ff
|
||||
*/
|
||||
GR_SWIG_BLOCK_MAGIC(op25, fsk4_demod_ff);
|
||||
|
||||
/*
|
||||
* Publicly-accesible default constuctor function for op25_fsk4_demod_bf.
|
||||
*/
|
||||
op25_fsk4_demod_ff_sptr op25_make_fsk4_demod_ff(gr::msg_queue::sptr queue, float sample_rate, float symbol_rate);
|
||||
|
||||
class op25_fsk4_demod_ff : public gr_block
|
||||
{
|
||||
private:
|
||||
op25_fsk4_demod_ff(gr::msg_queue::sptr queue, float sample_rate, float symbol_rate);
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------
|
||||
|
||||
/*
|
||||
* This does some behind-the-scenes magic so we can invoke
|
||||
* op25_make_slicer_fb from python as op25.slicer_fbf.
|
||||
*/
|
||||
GR_SWIG_BLOCK_MAGIC(op25, fsk4_slicer_fb);
|
||||
|
||||
/*
|
||||
* Publicly-accesible default constuctor function for op25_decoder_bf.
|
||||
*/
|
||||
op25_fsk4_slicer_fb_sptr op25_make_fsk4_slicer_fb(const std::vector<float> &slice_levels);
|
||||
|
||||
/*
|
||||
* The op25_fsk4_slicer block. Takes a series of float samples and
|
||||
* partitions them into dibit symbols according to the slices_levels
|
||||
* provided to the constructor.
|
||||
*/
|
||||
class op25_fsk4_slicer_fb : public gr_sync_block
|
||||
{
|
||||
private:
|
||||
op25_fsk4_slicer_fb (const std::vector<float> &slice_levels);
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------
|
||||
|
||||
/*
|
||||
* This does some behind-the-scenes magic so we can invoke
|
||||
* op25_make_decoder_bsf from python as op25.decoder_bf.
|
||||
*/
|
||||
GR_SWIG_BLOCK_MAGIC(op25, decoder_bf);
|
||||
|
||||
/*
|
||||
* Publicly-accesible default constuctor function for op25_decoder_bf.
|
||||
*/
|
||||
op25_decoder_bf_sptr op25_make_decoder_bf();
|
||||
|
||||
/**
|
||||
* The op25_decoder_bf block. Accepts a stream of dibit symbols and
|
||||
* produces an 8KS/s audio stream.
|
||||
*/
|
||||
class op25_decoder_bf : public gr_block
|
||||
{
|
||||
private:
|
||||
op25_decoder_bf();
|
||||
public:
|
||||
const char *destination() const;
|
||||
gr::msg_queue::sptr get_msgq() const;
|
||||
void set_msgq(gr::msg_queue::sptr msgq);
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------
|
||||
|
||||
/*
|
||||
* This does some behind-the-scenes magic so we can invoke
|
||||
* op25_make_pcap_source_b from python as op25.pcap_source_b.
|
||||
*/
|
||||
GR_SWIG_BLOCK_MAGIC(op25, pcap_source_b);
|
||||
|
||||
/*
|
||||
* Publicly-accesible constuctor function for op25_pcap_source.
|
||||
*/
|
||||
op25_pcap_source_b_sptr op25_make_pcap_source_b(const char *path, float delay);
|
||||
|
||||
/*
|
||||
* The op25_pcap_source block. Reads symbols from a tcpdump-formatted
|
||||
* file and produces a stream of symbols of the appropriate size.
|
||||
*/
|
||||
class op25_pcap_source_b : public gr_sync_block
|
||||
{
|
||||
private:
|
||||
op25_pcap_source_b(const char *path, float delay);
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------
|
|
@ -251,8 +251,8 @@ pngen23(uint32_t& Pr)
|
|||
* \param u0-u7 Result output vectors
|
||||
*/
|
||||
|
||||
static inline void
|
||||
imbe_header_decode(const voice_codeword& cw, uint32_t& u0, uint32_t& u1, uint32_t& u2, uint32_t& u3, uint32_t& u4, uint32_t& u5, uint32_t& u6, uint32_t& u7, uint32_t& E0, uint32_t& ET)
|
||||
static inline size_t
|
||||
imbe_header_decode(const voice_codeword& cw, uint32_t& u0, uint32_t& u1, uint32_t& u2, uint32_t& u3, uint32_t& u4, uint32_t& u5, uint32_t& u6, uint32_t& u7, uint32_t& E0, uint32_t& ET, bool bot_shift = true)
|
||||
{
|
||||
ET = 0;
|
||||
|
||||
|
@ -294,7 +294,10 @@ imbe_header_decode(const voice_codeword& cw, uint32_t& u0, uint32_t& u1, uint32_
|
|||
u6 = v6;
|
||||
|
||||
u7 = extract(cw, 137, 144);
|
||||
if (bot_shift)
|
||||
u7 <<= 1; /* so that bit0 is free (see note about BOT bit */
|
||||
|
||||
return errs;
|
||||
}
|
||||
|
||||
/* APCO IMBE header encoder.
|
||||
|
|
|
@ -24,32 +24,324 @@
|
|||
#include "voice_data_unit.h"
|
||||
#include "op25_imbe_frame.h"
|
||||
|
||||
#include <sstream>
|
||||
#include <map>
|
||||
#include <iostream>
|
||||
#include <boost/format.hpp>
|
||||
|
||||
#include <itpp/base/vec.h>
|
||||
#include <itpp/base/mat.h>
|
||||
#include <itpp/base/binary.h>
|
||||
#include <itpp/base/converters.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
voice_data_unit::~voice_data_unit()
|
||||
static void vec_mod(itpp::ivec& vec, int modulus = 2)
|
||||
{
|
||||
for (int i = 0; i < vec.length(); ++i)
|
||||
vec[i] = vec[i] % modulus;
|
||||
}
|
||||
|
||||
class cyclic_16_8_5_syndromes
|
||||
{
|
||||
public:
|
||||
typedef map<unsigned char,unsigned short> SyndromeTableMap;
|
||||
|
||||
const static itpp::imat cyclic_16_8_5;
|
||||
private:
|
||||
SyndromeTableMap m_syndrome_table;
|
||||
public:
|
||||
inline const SyndromeTableMap table() const
|
||||
{
|
||||
return m_syndrome_table;
|
||||
}
|
||||
|
||||
cyclic_16_8_5_syndromes(bool generate_now = false)
|
||||
{
|
||||
if (generate_now)
|
||||
generate();
|
||||
}
|
||||
|
||||
int generate()
|
||||
{
|
||||
if (m_syndrome_table.empty() == false)
|
||||
return -1;
|
||||
|
||||
// n=16, k=8
|
||||
|
||||
// E1
|
||||
itpp::ivec v("1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0");
|
||||
itpp::ivec r(cyclic_16_8_5 * v);
|
||||
vec_mod(r);
|
||||
itpp::bvec b(to_bvec(r));
|
||||
unsigned char ch = (unsigned char)bin2dec(b);
|
||||
itpp::bvec bV(to_bvec(v));
|
||||
unsigned short us = (unsigned short)bin2dec(bV);
|
||||
m_syndrome_table.insert(make_pair(ch, us));
|
||||
|
||||
// E2
|
||||
for (int i = 0; i <= (16 - 2); ++i)
|
||||
{
|
||||
itpp::ivec v2(v);
|
||||
v2[15-i] = 1;
|
||||
r = cyclic_16_8_5 * v2;
|
||||
bV = itpp::to_bvec(v2);
|
||||
|
||||
vec_mod(r);
|
||||
b = itpp::to_bvec(r);
|
||||
unsigned char ch = (unsigned char)itpp::bin2dec(b);
|
||||
unsigned short us = (unsigned short)itpp::bin2dec(bV);
|
||||
m_syndrome_table.insert(make_pair(ch, us));
|
||||
}
|
||||
|
||||
// E3 - disabled: min.d = 5, t=floor(5/2)=2
|
||||
/*for (int i = 0; i <= (16 - 2); ++i)
|
||||
{
|
||||
for (int j = 0; j < i; ++j)
|
||||
{
|
||||
ivec v3(v);
|
||||
v3[15-i] = 1;
|
||||
v3[15-j] = 1;
|
||||
r = cyclic_16_8_5 * v3;
|
||||
bV = to_bvec(v3);
|
||||
|
||||
vec_mod(r);
|
||||
b = to_bvec(r);
|
||||
unsigned char ch = (unsigned char)bin2dec(b);
|
||||
unsigned short us = (unsigned short)bin2dec(bV);
|
||||
m_syndrome_table.insert(make_pair(ch, us));
|
||||
}
|
||||
}*/
|
||||
|
||||
return m_syndrome_table.size();
|
||||
}
|
||||
};
|
||||
|
||||
const itpp::imat cyclic_16_8_5_syndromes::cyclic_16_8_5(
|
||||
"0 0 1 1 1 1 0 0 1 0 0 0 0 0 0 0;"
|
||||
"1 0 0 1 1 1 1 0 0 1 0 0 0 0 0 0;"
|
||||
"0 1 0 0 1 1 1 1 0 0 1 0 0 0 0 0;"
|
||||
"0 0 0 1 1 0 1 1 0 0 0 1 0 0 0 0;"
|
||||
"1 0 1 1 0 0 0 1 0 0 0 0 1 0 0 0;"
|
||||
"1 1 1 0 0 1 0 0 0 0 0 0 0 1 0 0;"
|
||||
"1 1 1 1 0 0 1 0 0 0 0 0 0 0 1 0;"
|
||||
"0 1 1 1 1 0 0 1 0 0 0 0 0 0 0 1"
|
||||
);
|
||||
|
||||
static cyclic_16_8_5_syndromes g_cyclic_16_8_5_syndromes(true);
|
||||
|
||||
static int decode_cyclic_16_8_5(const itpp::ivec& vec, itpp::ivec& out)
|
||||
{
|
||||
itpp::ivec vc(cyclic_16_8_5_syndromes::cyclic_16_8_5 * vec);
|
||||
vec_mod(vc);
|
||||
itpp::bvec vb(to_bvec(vc));
|
||||
|
||||
unsigned char ch = (unsigned char)itpp::bin2dec(vb);
|
||||
if (ch == 0x00)
|
||||
return 0;
|
||||
|
||||
const cyclic_16_8_5_syndromes::SyndromeTableMap& syndrome_table = g_cyclic_16_8_5_syndromes.table();
|
||||
cyclic_16_8_5_syndromes::SyndromeTableMap::const_iterator it = syndrome_table.find(ch);
|
||||
int j = 0;
|
||||
while (it == syndrome_table.end())
|
||||
{
|
||||
++j;
|
||||
vc = itpp::concat(itpp::ivec("0 0 0 0 0 0 0 0"), vc); // Restore to 16 bits
|
||||
vc.shift_left(vc[0]); // Rotate (s * x)
|
||||
vc = cyclic_16_8_5_syndromes::cyclic_16_8_5 * vc;
|
||||
vec_mod(vc);
|
||||
vb = itpp::to_bvec(vc);
|
||||
ch = (unsigned char)itpp::bin2dec(vb);
|
||||
it = syndrome_table.find(ch);
|
||||
|
||||
if (j >= 15)
|
||||
break;
|
||||
}
|
||||
|
||||
if (it == syndrome_table.end())
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned short us = it->second;
|
||||
itpp::bvec es(itpp::dec2bin(16, us));
|
||||
if (j > 0)
|
||||
es.shift_right(es.mid(16-j, j)); // e
|
||||
vb = itpp::to_bvec(vec);
|
||||
vb -= es;
|
||||
out = itpp::to_ivec(vb);
|
||||
|
||||
vc = cyclic_16_8_5_syndromes::cyclic_16_8_5 * out;
|
||||
vec_mod(vc);
|
||||
vb = itpp::to_bvec(vc);
|
||||
if (itpp::bin2dec(vb) != 0x00)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int decode_cyclic_16_8_5(itpp::ivec& vec)
|
||||
{
|
||||
return decode_cyclic_16_8_5(vec, vec);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
voice_data_unit::voice_data_unit(const_bit_queue& frame_body) :
|
||||
abstract_data_unit(frame_body)
|
||||
abstract_data_unit(frame_body),
|
||||
d_lsdw(0),
|
||||
d_lsdw_valid(false)
|
||||
{
|
||||
memset(d_lsd_byte_valid, 0x00, sizeof(d_lsd_byte_valid));
|
||||
}
|
||||
|
||||
voice_data_unit::~voice_data_unit()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
voice_data_unit::do_correct_errors(bit_vector& frame_body)
|
||||
{
|
||||
if (logging_enabled()) fprintf(stderr, "\n");
|
||||
|
||||
d_lsd_byte_valid[0] = d_lsd_byte_valid[1] = false;
|
||||
d_lsdw_valid = false;
|
||||
|
||||
itpp::ivec lsd1(16), lsd2(16);
|
||||
|
||||
for (int i = 0; i < 32; ++i)
|
||||
{
|
||||
int x = 1504 + i;
|
||||
x = x + ((x / 70) * 2); // Adjust bit index for status
|
||||
if (i < 16)
|
||||
lsd1[i] = frame_body[x];
|
||||
else
|
||||
lsd2[i-16] = frame_body[x];
|
||||
}
|
||||
|
||||
int iDecode1 = decode_cyclic_16_8_5(lsd1);
|
||||
if (iDecode1 >= 0)
|
||||
{
|
||||
d_lsd_byte_valid[0] = true;
|
||||
}
|
||||
else if (iDecode1 == -1)
|
||||
{
|
||||
// Error
|
||||
}
|
||||
int iDecode2 = decode_cyclic_16_8_5(lsd2);
|
||||
if (iDecode2 >= 0)
|
||||
{
|
||||
d_lsd_byte_valid[1] = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Error
|
||||
}
|
||||
|
||||
d_lsdw = 0;
|
||||
for (int i = 0; i < 8; ++i)
|
||||
d_lsdw = d_lsdw | (lsd1[i] << (7 - i)); // Little-endian byte swap
|
||||
for (int i = 0; i < 8; ++i)
|
||||
d_lsdw = d_lsdw | (lsd2[i] << (15 - i)); // Little-endian byte swap
|
||||
|
||||
if (d_lsd_byte_valid[0] && d_lsd_byte_valid[1])
|
||||
d_lsdw_valid = true;
|
||||
}
|
||||
|
||||
uint16_t
|
||||
voice_data_unit::lsdw() const
|
||||
{
|
||||
return d_lsdw;
|
||||
}
|
||||
|
||||
bool
|
||||
voice_data_unit::lsdw_valid() const
|
||||
{
|
||||
return d_lsdw_valid;
|
||||
}
|
||||
|
||||
static void extract(unsigned int u, size_t n, std::vector<bool>& out)
|
||||
{
|
||||
for (size_t i = 0; i < n; ++i)
|
||||
out.push_back(((u & (1 << (n-1-i))) != 0));
|
||||
}
|
||||
|
||||
void
|
||||
voice_data_unit::do_decode_audio(const_bit_vector& frame_body, imbe_decoder& imbe)
|
||||
voice_data_unit::do_decode_audio(const_bit_vector& frame_body, imbe_decoder& imbe, crypto_module::sptr crypto_mod)
|
||||
{
|
||||
voice_codeword cw(voice_codeword_sz);
|
||||
for(size_t i = 0; i < nof_voice_codewords; ++i) {
|
||||
imbe_deinterleave(frame_body, cw, i);
|
||||
|
||||
unsigned int u0 = 0;
|
||||
unsigned int u1,u2,u3,u4,u5,u6,u7;
|
||||
unsigned int E0 = 0;
|
||||
unsigned int ET = 0;
|
||||
|
||||
// PN/Hamming/Golay - etc.
|
||||
size_t errs = imbe_header_decode(cw, u0, u1, u2, u3, u4, u5, u6, u7, E0, ET, false); // E0 & ET are not used, and are always returned as 0
|
||||
|
||||
crypto_algorithm::sptr algorithm;
|
||||
if (crypto_mod)
|
||||
algorithm = crypto_mod->current_algorithm();
|
||||
|
||||
if (algorithm)
|
||||
{
|
||||
if (i == 8)
|
||||
{
|
||||
d_lsdw ^= algorithm->generate(16); // LSDW
|
||||
}
|
||||
|
||||
u0 ^= (int)algorithm->generate(12);
|
||||
u1 ^= (int)algorithm->generate(12);
|
||||
u2 ^= (int)algorithm->generate(12);
|
||||
u3 ^= (int)algorithm->generate(12);
|
||||
|
||||
u4 ^= (int)algorithm->generate(11);
|
||||
u5 ^= (int)algorithm->generate(11);
|
||||
u6 ^= (int)algorithm->generate(11);
|
||||
|
||||
u7 ^= (int)algorithm->generate(7);
|
||||
|
||||
imbe_header_encode(cw, u0, u1, u2, u3, u4, u5, u6, (u7 << 1));
|
||||
}
|
||||
|
||||
std::vector<bool> cw_raw;
|
||||
extract(u0, 12, cw_raw);
|
||||
extract(u1, 12, cw_raw);
|
||||
extract(u2, 12, cw_raw);
|
||||
extract(u3, 12, cw_raw);
|
||||
extract(u4, 11, cw_raw);
|
||||
extract(u5, 11, cw_raw);
|
||||
extract(u6, 11, cw_raw);
|
||||
extract(u7, 7, cw_raw);
|
||||
|
||||
const int cw_octets = 11;
|
||||
|
||||
std::vector<uint8_t> cw_vector(cw_octets);
|
||||
extract(cw_raw, 0, (cw_octets * 8), &cw_vector[0]);
|
||||
|
||||
if (logging_enabled())
|
||||
{
|
||||
std::stringstream ss;
|
||||
for (size_t n = 0; n < cw_vector.size(); ++n)
|
||||
{
|
||||
ss << (boost::format("%02x") % (int)cw_vector[n]);
|
||||
if (n < (cw_vector.size() - 1))
|
||||
ss << " ";
|
||||
}
|
||||
|
||||
if (errs > 0)
|
||||
ss << (boost::format(" (%llu errors)") % errs);
|
||||
|
||||
std:cerr << (boost::format("%s:\t%s") % duid_str() % ss.str()) << std::endl;
|
||||
}
|
||||
|
||||
imbe.decode(cw);
|
||||
}
|
||||
|
||||
if (logging_enabled()) fprintf(stderr, "%s: LSDW: 0x%04x, %s\n", duid_str().c_str(), d_lsdw, (d_lsdw_valid ? "valid" : "invalid"));
|
||||
}
|
||||
|
||||
uint16_t
|
||||
|
|
|
@ -38,6 +38,17 @@ public:
|
|||
*/
|
||||
virtual ~voice_data_unit();
|
||||
|
||||
const static int LSD_BYTE_COUNT=2;
|
||||
|
||||
private:
|
||||
|
||||
union {
|
||||
uint8_t d_lsd_byte[LSD_BYTE_COUNT];
|
||||
uint16_t d_lsdw;
|
||||
};
|
||||
bool d_lsdw_valid;
|
||||
bool d_lsd_byte_valid[LSD_BYTE_COUNT];
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
|
@ -63,7 +74,7 @@ protected:
|
|||
* \param frame_body The const_bit_vector to decode.
|
||||
* \param imbe The imbe_decoder to use to generate the audio.
|
||||
*/
|
||||
virtual void do_decode_audio(const_bit_vector& frame_body, imbe_decoder& imbe);
|
||||
virtual void do_decode_audio(const_bit_vector& frame_body, imbe_decoder& imbe, crypto_module::sptr crypto_mod);
|
||||
|
||||
/**
|
||||
* Returns the expected size (in bits) of this data_unit. For
|
||||
|
@ -74,6 +85,10 @@ protected:
|
|||
*/
|
||||
virtual uint16_t frame_size_max() const;
|
||||
|
||||
virtual uint16_t lsdw() const;
|
||||
|
||||
virtual bool lsdw_valid() const;
|
||||
|
||||
};
|
||||
|
||||
#endif /* INCLUDED_VOICE_DATA_UNIT_H */
|
||||
|
|
|
@ -28,9 +28,10 @@
|
|||
|
||||
using namespace std;
|
||||
|
||||
voice_du_handler::voice_du_handler(data_unit_handler_sptr next, imbe_decoder_sptr decoder) :
|
||||
voice_du_handler::voice_du_handler(data_unit_handler_sptr next, imbe_decoder_sptr decoder, crypto_module::sptr crypto_mod) :
|
||||
data_unit_handler(next),
|
||||
d_decoder(decoder)
|
||||
d_decoder(decoder),
|
||||
d_crypto_mod(crypto_mod)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -42,6 +43,6 @@ voice_du_handler::~voice_du_handler()
|
|||
void
|
||||
voice_du_handler::handle(data_unit_sptr du)
|
||||
{
|
||||
du->decode_audio(*d_decoder);
|
||||
du->decode_audio(*d_decoder, d_crypto_mod);
|
||||
data_unit_handler::handle(du);
|
||||
}
|
||||
|
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
#include "data_unit_handler.h"
|
||||
#include "imbe_decoder.h"
|
||||
#include "crypto.h"
|
||||
|
||||
#include <boost/noncopyable.hpp>
|
||||
|
||||
|
@ -43,7 +44,7 @@ public:
|
|||
* \param next The next data_unit_handler in the chain.
|
||||
* \param decoder An imbe_decoder_sptr to the IMBE decoder to use.
|
||||
*/
|
||||
voice_du_handler(data_unit_handler_sptr next, imbe_decoder_sptr decoder);
|
||||
voice_du_handler(data_unit_handler_sptr next, imbe_decoder_sptr decoder, crypto_module::sptr crypto_mod = crypto_module::sptr()); // TODO: Add capability to decoder_ff (remove default argument)
|
||||
|
||||
/**
|
||||
* voice_du_handler virtual destructor.
|
||||
|
@ -64,6 +65,8 @@ private:
|
|||
*/
|
||||
imbe_decoder_sptr d_decoder;
|
||||
|
||||
crypto_module::sptr d_crypto_mod;
|
||||
|
||||
};
|
||||
|
||||
#endif /* INCLUDED_VOICE_DU_HANDLER_H */
|
||||
|
|
|
@ -1,5 +1,7 @@
|
|||
/* -*- c++ -*- */
|
||||
|
||||
%include "pycontainer.swg"
|
||||
|
||||
#define OP25_API
|
||||
|
||||
%include "gnuradio.i" // the common stuff
|
||||
|
@ -15,6 +17,11 @@
|
|||
#include "op25/pcap_source_b.h"
|
||||
%}
|
||||
|
||||
%template(key_type) std::vector<unsigned char>;
|
||||
// This causes SWIG to segfault
|
||||
//%template(key_map_type) std::map<uint16_t,key_type >;
|
||||
%template(key_map_type) std::map<uint16_t,std::vector<unsigned char> >;
|
||||
|
||||
%include "op25/fsk4_demod_ff.h"
|
||||
GR_SWIG_BLOCK_MAGIC2(op25, fsk4_demod_ff);
|
||||
%include "op25/fsk4_slicer_fb.h"
|
||||
|
|
|
@ -83,7 +83,6 @@ set(GRC_BLOCKS_DIR ${GR_PKG_DATA_DIR}/grc/blocks)
|
|||
########################################################################
|
||||
# Find gnuradio build dependencies
|
||||
########################################################################
|
||||
find_package(GnuradioRuntime)
|
||||
find_package(CppUnit)
|
||||
|
||||
# To run a more advanced search for GNU Radio and it's components and
|
||||
|
@ -91,8 +90,9 @@ find_package(CppUnit)
|
|||
# of GR_REQUIRED_COMPONENTS (in all caps) and change "version" to the
|
||||
# minimum API compatible version required.
|
||||
#
|
||||
# set(GR_REQUIRED_COMPONENTS RUNTIME BLOCKS FILTER ...)
|
||||
set(GR_REQUIRED_COMPONENTS RUNTIME BLOCKS FILTER PMT)
|
||||
# find_package(Gnuradio "version")
|
||||
find_package(Gnuradio)
|
||||
|
||||
if(NOT GNURADIO_RUNTIME_FOUND)
|
||||
message(FATAL_ERROR "GnuRadio Runtime required to compile op25_repeater")
|
||||
|
|
Loading…
Reference in New Issue