蓝牙调频算法

#include <stdio.h>
#include <stdint.h>
#include <sys/types.h>
/* sequence type */
typedef enum {
SEQ_PAGE_SCAN,
SEQ_INQUIRY_SCAN,
SEQ_PAGE,
SEQ_INQUIRY,
SEQ_PERIPHERAL_PAGE_RESP,
SEQ_CENTRAL_PAGE_RESP,
SEQ_INQUIRY_RESP,
SEQ_BASIC,
SEQ_ADAPTED,
} seq_type_t;

uint8_t AFH_channel_map[10] = {
// 7 —- 0 + i * 8, i = 1,2,3…9
0xff, // 0b11111111, /* elements 0 – 7 */
0xff, // 0b11111111, /* elements 8 – 15 */
0xff, // 0b11111111, /* elements 16 – 23 */
0xff, // 0b11111111, /* elements 24 – 31 */
0xff, // 0b11111111, /* elements 32 – 39 */
0xff, // 0b11111111, /* elements 40 – 47 */
0xff, // 0b11111111, /* elements 48 – 55 */
0xff, // 0b11111111, /* elements 56 – 63 */
0xff, // 0b11111111, /* elements 64 – 71 */
0x7f, // 0b01111111 /* elements 72 – 79, 79 is reserved */
};
static uint32_t register_bank[79] = {0};
static uint8_t used_chan_num = 79; // AFH_channel_map 中的可用信道数量
static uint32_t selection_kernel(uint32_t X, uint32_t A, uint32_t B, uint32_t C,
uint32_t D, uint32_t E, uint32_t F, uint32_t Y1,
uint32_t Y2, uint32_t *p5_out)
{
uint32_t Z_Prime, Z, perm5out;
uint32_t P;
uint32_t K;

/* Refer Core Specification v5.3, Vol 2, Part B, Section 2.6.2.3 */
uint32_t permt_stage[14][2] = {{1, 2}, {0, 3},
{1, 3}, {2, 4},
{0, 3}, {1, 4},
{3, 4}, {0, 2},
{1, 3}, {0, 4},
{3, 4}, {1, 2},
{2, 3}, {0, 1}};

/* first add operation */
Z_Prime = (X + A) % 32u ;

/* xor operation */
Z = ((Z_Prime & 0xf) ^ B) | (Z_Prime & 0x10);

/* Permutation operation */
P = D | ((C ^ (Y1 ? 0x1f : 0)) << 9u);
perm5out = Z;

for (int i = 0; i < 14; i++) {
if (P >> (13 – i) & 1) {
if (Z & (1u << permt_stage[i][0]))
perm5out |= (1u << permt_stage[i][1]);
else
perm5out &= ~(1u << permt_stage[i][1]);

if (Z & (1u << permt_stage[i][1]))
perm5out |= (1u << permt_stage[i][0]);
else
perm5out &= ~(1u << permt_stage[i][0]);
Z = perm5out;
};
}

/* 记录perm5out, 为可能的重映射使用 */
*p5_out = perm5out;

/* second add operation, return channel index */
K = (perm5out + E + F + Y2) % 79u;
return register_bank[K];
}

static uint32_t bt_hop_chan_remapping(uint32_t K)
{
uint32_t j = 0;
for (int i = 0; i < 79; i++) {
uint32_t idx = register_bank[i];
if ((AFH_channel_map[idx / 8] & (1 << (idx % 8))) &&
j++ == K) {
return register_bank[i];
}
}

/* never arrived here */
return 99;
}

uint32_t bt_hop_selection(seq_type_t type, uint32_t BD_ADDR,
uint32_t input_clk, uint32_t frozen_clk,
uint32_t counter, uint32_t koffset,
uint32_t knudge)
{
uint32_t X, A, B, C, E, F, F_Prime, perm5out;
uint32_t D;
uint32_t Y1 = 0, Y2 = 0;
uint32_t K, channel_index = 0;

/* prepare input parameters */
A = (BD_ADDR >> 23u) & 0x1f;
B = (BD_ADDR >> 19u) & 0xf;
C = (BD_ADDR & 1u) | ((BD_ADDR >> 1u) & 2u) |
((BD_ADDR >> 2u) & 4u) | ((BD_ADDR >> 3u) & 8u) |
((BD_ADDR >> 4u) & 16u);
D = (BD_ADDR >> 10u) & 0x1ff;
E = ((BD_ADDR >> 1u) & 1u) | ((BD_ADDR >> 2u) & 2u) |
((BD_ADDR >> 3u) & 4u) | ((BD_ADDR >> 4u) & 8u) |
((BD_ADDR >> 5u) & 16u) | ((BD_ADDR >> 6u) & 32u) |
((BD_ADDR >> 7u) & 64u);
F = 0u;
switch(type) {
case SEQ_PAGE_SCAN:
/* channel changed per 1.28s */
X = (input_clk >> 12u) & 0x1f;
break;
case SEQ_INQUIRY_SCAN:
X = (((input_clk >> 12u) & 0x1f) + counter) % 32u;
break;
case SEQ_PAGE:
X = (((input_clk >> 12u) & 0x1f) + koffset + knudge +
(((((input_clk >> 1u) & 0xe) | (input_clk & 1u)) –
((input_clk >> 12u) & 0x1f) + 32u) % 16u)) % 32u;
Y1 = (input_clk >> 1u) & 1u;
Y2 = Y1 * 32u;
break;
case SEQ_INQUIRY:
X = (((input_clk >> 12u) & 0x1f) + koffset + knudge +
(((((input_clk >> 1u) & 0xe) | (input_clk & 1u)) –
((input_clk >> 12u) & 0x1f)) % 16u)) % 32u;
Y1 = (input_clk >> 1u) & 1u;
Y2 = Y1 * 32u;
break;
case SEQ_PERIPHERAL_PAGE_RESP:
X = (((frozen_clk >> 12u) & 0x1fu) + counter) % 32u;
Y1 = (input_clk >> 1u) & 1u;
Y2 = Y1 * 32u;
break;
case SEQ_CENTRAL_PAGE_RESP:
X = (((frozen_clk >> 12u) & 0x1f) + koffset + knudge +
(((((frozen_clk >> 1u) & 0xe) | (frozen_clk & 1u)) –
((frozen_clk >> 12u) & 0x1f)) % 16u) + counter) % 32u;
Y1 = (input_clk >> 1u) & 1u;
Y2 = Y1 * 32u;
break;
case SEQ_INQUIRY_RESP:
X = (((input_clk >> 12u) & 0x1f) + counter) % 32u;
Y1 = 1u;
Y2 = 32u;
break;
case SEQ_BASIC:
case SEQ_ADAPTED:
X = (input_clk >> 2u) & 0x1f;
Y1 = (input_clk >> 1u) & 1u;
Y2 = Y1 * 32u;
A = A ^ ((input_clk >> 21u) & 0x1f);
C = C ^ ((input_clk >> 16u) & 0x1f);
D = D ^ ((input_clk >> 7u) & 0x1ff);
F = (16u * ((input_clk >> 7u) & 0x1fffff)) % 79u;
break;
}

/* basic selection */
channel_index = selection_kernel(X, A, B, C, D, E, F, Y1, Y2, &perm5out);

/* remapping */
if (type == SEQ_ADAPTED &&
!(AFH_channel_map[channel_index / 8] &
(1 << (channel_index % 8)))) {
/* calcu F_Prime */
F_Prime = (16 * ((input_clk >> 7) & 0x1fffff)) % used_chan_num;
/* add & mod operation, re-calcu K */
K = (perm5out + E + F_Prime + Y2) % used_chan_num;
/* remap */
channel_index = bt_hop_chan_remapping(K);
}

return channel_index;
}

int main()
{
int i = 0, j = 0;
uint32_t channel;
for (i = 0; i < 79; i++)
{
if (i <= (79/2))
register_bank[i] = i * 2;
else
register_bank[i] = i – (79/2 – (j++));
}
for (i = 0; i < 79; i++)
{
printf(“%d “, register_bank[i]);
}
channel = bt_hop_selection(SEQ_PAGE, 0, 1,0,0,24,0);
printf(“channel :%d\n”,channel);
return 0;

}

本文为原创文章，转载请注明出处！

打赏作者