jens/radio
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
e1ac9fb3f3d0b007d7f7a94e702df2330c7760ee
radio/frame.c
T
jens 290fd5f5e0 Initial import 
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/radio@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
2014-07-19 07:44:42 +00:00

357 lines
7.7 KiB
C
Executable File
Raw Blame History

// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include <crc/crc.h>
#include "symbol.h"
#include "frame.h"
// --------------------------------------------------------------
uint32_t Scramble(uint8_t *pSrcDst, uint32_t state, uint32_t poly, uint32_t len)
{
uint32_t i;
while(len)
{
*pSrcDst ^= (uint8_t)state;
pSrcDst++;
len--;
for (i=0; i < 8; i++)
{
if (state & 0x0001)
state = (state >> 1) ^ poly;
else
state = (state >> 1);
}
}
return state;
}
uint32_t FrameCalcNumStreamBytes(uint32_t len_raw)
{
uint32_t len, num_frames;
num_frames = (uint32_t)ceil((float)len_raw / FRAME_NUM_BYTES_PER_FRAME);
len = num_frames*sizeof(frame_t);
if (len < sizeof(frame_t))
len = sizeof(frame_t);
return len;
}
uint32_t FrameFormat(uint8_t *pRaw, uint8_t *pFormatted, uint32_t len_raw, uint32_t *pPrn_state, uint16_t frame_type)
{
frame_t frame;
uint32_t bytes_remain, len_formatted, len;
uint32_t prn_state;
if (len_raw == 0)
return 0;
prn_state = *pPrn_state;
// Set preamble
frame.hdr.preamble[0] = FRAME_PREAMBLE_IV;
frame.hdr.preamble[1] = ~(frame.hdr.preamble[0]);
bytes_remain = len_raw;
len_formatted = 0;
while(bytes_remain)
{
len = FRAME_NUM_BYTES_PER_FRAME;
if (bytes_remain < FRAME_NUM_BYTES_PER_FRAME)
len = bytes_remain;
memset(frame.data, 0, FRAME_NUM_BYTES_PER_FRAME);
memcpy(frame.data, pRaw, len);
frame.hdr.crc16 = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, frame.data, FRAME_NUM_BYTES_PER_FRAME);
frame.hdr.type = frame_type;
frame.hdr.prn_state = prn_state;
frame.hdr.length = (uint16_t)len;
prn_state = Scramble(frame.data, prn_state, FRAME_SCRAMBLER_POLY, FRAME_NUM_BYTES_PER_FRAME);
prn_state = Scramble((uint8_t*)&frame.hdr.crc16, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.crc16));
prn_state = Scramble((uint8_t*)&frame.hdr.type, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.type));
prn_state = Scramble((uint8_t*)&frame.hdr.length, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.length));
pRaw += len;
bytes_remain -= len;
memcpy(pFormatted, &frame, sizeof(frame_t));
pFormatted += sizeof(frame_t);
len_formatted += sizeof(frame_t);
}
*pPrn_state = prn_state;
return len_formatted;
}
uint32_t FrameDeformat(uint8_t *pFormatted, uint8_t *pRaw, uint32_t len_formatted, uint16_t frame_type_mask)
{
frame_hdr_t hdr;
uint32_t bytes_remain, len_raw, sync_state, frame_sync, i;
uint16_t crc16_ist;
uint8_t temp[FRAME_NUM_BYTES_PER_FRAME];
uint32_t prn_state;
if (len_formatted == 0)
return 0;
i = 0;
bytes_remain = len_formatted;
len_raw = 0;
while(bytes_remain)
{
sync_state = 0;
do
{
// Find frame boundaries
switch(sync_state)
{
case 0:
if (pFormatted[i] == FRAME_PREAMBLE_IV)
{
sync_state = 1;
}
break;
case 1:
if ((uint8_t)(~pFormatted[i]) == pFormatted[i-1])
{
sync_state = 2;
}
else
{
sync_state = 0;
}
break;
}
i++;
bytes_remain--;
frame_sync = (sync_state == 2);
} while(bytes_remain && !frame_sync);
memcpy(&hdr, &pFormatted[i-2], sizeof(frame_hdr_t));
memcpy(temp, &pFormatted[i-2+sizeof(frame_hdr_t)], FRAME_NUM_BYTES_PER_FRAME);
prn_state = Scramble(temp, hdr.prn_state, FRAME_SCRAMBLER_POLY, FRAME_NUM_BYTES_PER_FRAME);
prn_state = Scramble((uint8_t*)&hdr.crc16, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.crc16));
prn_state = Scramble((uint8_t*)&hdr.type, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.type));
Scramble((uint8_t*)&hdr.length, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.length));
crc16_ist = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, temp, FRAME_NUM_BYTES_PER_FRAME);
if (crc16_ist == hdr.crc16)
{
if ((hdr.type & frame_type_mask) == frame_type_mask)
{
memcpy(&pRaw[len_raw], temp, hdr.length);
len_raw += hdr.length;
}
i += FRAME_NUM_BYTES_PER_FRAME + sizeof(frame_hdr_t) - 2;
bytes_remain -= (FRAME_NUM_BYTES_PER_FRAME + sizeof(frame_hdr_t) - 2);
}
if ((int32_t)bytes_remain < sizeof(frame_t))
bytes_remain = 0;
}
return len_raw;
}
// Transmission order MSB first
uint32_t FrameSerialize(uint8_t *pSrc, uint8_t *pDst, uint32_t nBitsPerSym, uint32_t srclen)
{
uint32_t bits_remain, i, j;
uint8_t src, dst;
if (srclen == 0)
return 0;
if (nBitsPerSym > 8)
return (uint32_t)-1;
j = 0;
src = *pSrc;
bits_remain = 8;
while(srclen)
{
dst = 0;
for (i=0; i < nBitsPerSym; i++)
{
dst <<= 1;
dst |= (0x01 & (src >> 7));
src <<= 1;
bits_remain--;
if (bits_remain == 0)
{
pSrc++;
bits_remain = 8;
src = *pSrc;
srclen--;
}
}
pDst[j++] = dst;
}
return j;
}
uint32_t FrameDeSerialize(uint8_t *pSrc, uint8_t *pDst, uint32_t nBitsPerSym, uint32_t srclen, uint16_t frame_type_mask)
{
uint32_t size, err, bits_remain, i, j, byte_sync, srcRemain, bitStart, numCorrupt;
uint8_t src, dst, *pBitStart;
uint16_t dst16, t;
uint8_t frame[sizeof(frame_t)], data[FRAME_NUM_BYTES_PER_FRAME];
if (srclen == 0)
return 0;
if (nBitsPerSym > 8)
return (uint32_t)-1;
src = *pSrc;
bits_remain = nBitsPerSym;
bitStart = bits_remain;
pBitStart = pSrc;
numCorrupt = 0;
t = (FRAME_PREAMBLE_IV << 8) | (0xFF & ~FRAME_PREAMBLE_IV);
size = 0;
do
{
j = 0;
dst16 = 0;
byte_sync = 0;
do
{
bits_remain--;
dst16 <<= 1;
dst16 |= (0x01 & (src >> (bits_remain)));
if (bits_remain == 0)
{
pSrc++;
bits_remain = nBitsPerSym;
src = *pSrc;
srclen--;
if (!srclen)
break;
}
// Find frame boundaries
if (dst16 == t)
{
pBitStart = pSrc;
bitStart = bits_remain;
srcRemain = srclen;
byte_sync = 1;
frame[j++] = 0xFF & (t >> 8);
frame[j++] = 0xFF & t;
}
} while(srclen && !byte_sync);
if (!srclen)
break;
for(j; j < sizeof(frame_t); j++)
{
dst = 0;
for (i=0; i < 8; i++)
{
bits_remain--;
dst <<= 1;
dst |= (0x01 & (src >> (bits_remain)));
if (bits_remain == 0)
{
pSrc++;
bits_remain = nBitsPerSym;
src = *pSrc;
srclen--;
if (!srclen)
break;
}
}
frame[j] = dst;
if (!srclen)
break;
}
if (!srclen)
break;
err = FrameDeformat(frame, data, sizeof(frame_t), frame_type_mask);
if (err > 0)
{
memcpy(&pDst[size], frame, sizeof(frame_t));
size += sizeof(frame_t);
}
else
{
if (size > 0)
numCorrupt++;
bits_remain = bitStart;
pSrc = pBitStart;
srclen = srcRemain;
}
} while(srclen);
// printf("Number of corrupted frames = %d\n", numCorrupt);
return size;
}
// Differential M-PSK symbol mapping
uint32_t FrameSymbolsMap(uint8_t *pBits, cpx_t *pSym, uint32_t nBitsPerSym, uint32_t num_syms, uint32_t type)
{
uint32_t i;
sym_map_t sym_mapper;
if (num_syms == 0)
return 0;
SymMapInit(&sym_mapper, nBitsPerSym, type);
for(i=0; i < num_syms; i++)
{
pSym[i] = SymMapEncode(&sym_mapper, pBits[i]);
}
return num_syms;
}
// Differential M-PSK symbol de-mapping
uint32_t FrameSymbolsDemap(cpx_t *pSym, uint8_t *pBits, uint32_t nBitsPerSym, uint32_t num_syms, uint32_t type)
{
uint32_t i;
sym_map_t sym_demapper;
sym_err_t sym_err;
if (num_syms == 0)
return 0;
memset(&sym_err, 0, sizeof(sym_err_t));
SymMapInit(&sym_demapper, nBitsPerSym, type);
for(i=0; i < num_syms; i++)
{
pBits[i] = SymMapDecode(&sym_demapper, pSym[i], &sym_err);
}
return num_syms;
}
// --------------------------------------------------------------
Reference in New Issue View Git Blame Copy Permalink