diff options
Diffstat (limited to 'src/target/firmware')
25 files changed, 8589 insertions, 84 deletions
diff --git a/src/target/firmware/Makefile b/src/target/firmware/Makefile index 914db6c7..24fbea28 100644 --- a/src/target/firmware/Makefile +++ b/src/target/firmware/Makefile @@ -136,7 +136,7 @@ INCLUDES=-Iinclude/ -I../../../include -I../../shared/libosmocore/include -I../. # # Uncomment this line if you want to enable Tx (Transmit) Support. -#CFLAGS += -DCONFIG_TX_ENABLE +CFLAGS += -DCONFIG_TX_ENABLE # Uncomment this line if you want to write to flash. #CFLAGS += -DCONFIG_FLASH_WRITE diff --git a/src/target/firmware/Makefile.mtk b/src/target/firmware/Makefile.mtk index 927e31a0..30fa2fcf 100644 --- a/src/target/firmware/Makefile.mtk +++ b/src/target/firmware/Makefile.mtk @@ -4,16 +4,14 @@ BOARDS?=mt62xx # List of all applications (meant to be overridden on command line) APPLICATIONS?=loader_mtk -APP_loader_mtk_ENVIRONMENTS=mtkram - -ENV_mtkram_LDS=board/mediatek/ram.lds -ENV_mtkram_OBJS=board/mediatek/start.ram.o +mtkram_LDS=board/mediatek/ram.lds +mtkram_OBJS=board/mediatek/start.ram.o mtk_COMMON_OBJS=board/mediatek/uart.o # Mediatek MT62xx -BOARD_mt62xx_OBJS=$(mtk_COMMON_OBJS) board/mt62xx/init.o -BOARD_mt62xx_ENVIRONMENTS=mtkram +mt62xx_OBJS=$(mtk_COMMON_OBJS) board/mt62xx/init.o +mt62xx_ENVIRONMENTS=mtkram # Global include path INCLUDES=-Iinclude/ -I../../../include -I../../shared/libosmocore/include diff --git a/src/target/firmware/include/layer1/async.h b/src/target/firmware/include/layer1/async.h index de996a67..b5cc2516 100644 --- a/src/target/firmware/include/layer1/async.h +++ b/src/target/firmware/include/layer1/async.h @@ -33,7 +33,7 @@ int l1a_txq_msgb_count(struct llist_head *queue); void l1a_txq_msgb_flush(struct llist_head *queue); /* request a RACH */ -void l1a_rach_req(uint16_t offset, uint8_t combined, uint8_t ra); +void l1a_rach_req(uint16_t offset, uint8_t combined, uint8_t ra, uint16_t arfcn); /* schedule frequency change */ void l1a_freq_req(uint32_t fn_sched); diff --git a/src/target/firmware/include/layer1/mframe_sched.h b/src/target/firmware/include/layer1/mframe_sched.h index 74e2d271..3c9aa8cb 100644 --- a/src/target/firmware/include/layer1/mframe_sched.h +++ b/src/target/firmware/include/layer1/mframe_sched.h @@ -4,6 +4,8 @@ #include <stdint.h> enum mframe_task { + MF_TASK_TEST1, + MF_TASK_BCCH_NORM, MF_TASK_BCCH_EXT, MF_TASK_CCCH, diff --git a/src/target/firmware/include/layer1/prim.h b/src/target/firmware/include/layer1/prim.h index 30c51ae6..5c2cb963 100644 --- a/src/target/firmware/include/layer1/prim.h +++ b/src/target/firmware/include/layer1/prim.h @@ -20,7 +20,7 @@ void l1s_nb_test(uint8_t base_fn); void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req); void l1a_freq_req(uint32_t fn_sched); -void l1a_rach_req(uint16_t offset, uint8_t combined, uint8_t ra); +void l1a_rach_req(uint16_t offset, uint8_t combined, uint8_t ra, uint16_t arfcn); /* Primitives raw scheduling sets */ extern const struct tdma_sched_item nb_sched_set[]; @@ -29,6 +29,10 @@ extern const struct tdma_sched_item nb_sched_set_ul[]; extern const struct tdma_sched_item tch_sched_set[]; extern const struct tdma_sched_item tch_a_sched_set[]; extern const struct tdma_sched_item tch_d_sched_set[]; -extern const struct tdma_sched_item neigh_pm_sched_set[]; +extern const struct tdma_sched_item neigh_pm_idle_sched_set[]; +extern const struct tdma_sched_item neigh_pm_tch_sched_set[]; +extern const struct tdma_sched_item neigh_sync_sched_set[]; + +extern const struct tdma_sched_item test1_sched[]; #endif /* _L1_PRIM_H */ diff --git a/src/target/firmware/include/layer1/sync.h b/src/target/firmware/include/layer1/sync.h index 3565ee20..df3c6fdc 100644 --- a/src/target/firmware/include/layer1/sync.h +++ b/src/target/firmware/include/layer1/sync.h @@ -55,6 +55,17 @@ struct l1s_h1 { }; struct l1s_state { + + //MTZ + int new_dm; + uint32_t orig_tpu_offset; + uint32_t tpu_offsets[64]; + uint32_t nb_freq_diff[64]; + uint32_t nb_frame_diff[64]; + int32_t nb_sb_freq_diff[64]; + uint32_t nb_sb_snr[64]; + uint16_t tpu_offsets_arfcn[64]; + struct gsm_time current_time; /* current GSM time */ struct gsm_time next_time; /* GSM time at next TMDMA irq */ @@ -127,6 +138,7 @@ struct l1s_state { GSM_DCHAN_UNKNOWN, } type; + uint8_t chan_nr; uint8_t scn; uint8_t tsc; uint8_t tn; @@ -145,19 +157,37 @@ struct l1s_state { struct l1s_h0 st_h0; struct l1s_h1 st_h1; }; + + uint8_t rx_only; } dedicated; /* neighbour cell power measurement process */ struct { - uint8_t n, second; - uint8_t pos; - uint8_t running; - uint16_t band_arfcn[64]; - uint8_t tn[64]; - uint8_t level[64]; + uint32_t start_fn; /* frame number of measumrement start */ + uint8_t valid; /* we have a complete set of measurements */ + uint8_t rounds; /* current rounds of complete measurements */ + uint8_t pos; /* current neighbor to measure */ + uint8_t running; /* DSP task running */ + uint8_t n; /* number of neighbors to measure */ + uint16_t band_arfcn[64]; /* list of ARFCNs */ + uint8_t tn[64]; /* list of TS offset for each measurement */ + uint16_t level_sum[64]; /* sum while processing rounds */ + uint8_t level[64]; /* latest results */ + uint32_t tpu_offset[64]; } neigh_pm; + + /* neighbor cell SCH sync process */ + struct { + uint8_t flags_bsic[64]; /* flags + bsic */ + uint8_t count; /* counter for sync process */ + uint8_t index; /* cell of current sync process (0..63) */ + uint8_t running; /* DSP task running */ + } neigh_sb; }; +#define NEIGH_PM_FLAG_SCANNED 0x80 +#define NEIGH_PM_FLAG_BSIC 0x40 + extern struct l1s_state l1s; struct l1s_meas_hdr { diff --git a/src/target/firmware/include/layer1/tpu_window.h b/src/target/firmware/include/layer1/tpu_window.h index 7b146f12..287d8962 100644 --- a/src/target/firmware/include/layer1/tpu_window.h +++ b/src/target/firmware/include/layer1/tpu_window.h @@ -6,6 +6,8 @@ enum l1_rxwin_type { L1_RXWIN_FB, /* FCCH burst detection */ L1_RXWIN_SB, /* SCH burst detection */ L1_RXWIN_NB, /* Normal burst decoding */ + L1_RXWIN_FB26, /* SCH burst detection of neighbour cell */ + L1_RXWIN_SB26, /* SCH burst detection of neighbour cell */ _NUM_L1_RXWIN }; diff --git a/src/target/firmware/layer1/afc.c b/src/target/firmware/layer1/afc.c index a51a1071..bf47a6e4 100644 --- a/src/target/firmware/layer1/afc.c +++ b/src/target/firmware/layer1/afc.c @@ -93,7 +93,10 @@ void afc_correct(int16_t freq_error, uint16_t arfcn) } delta = (int16_t) ((afc_norm_factor * (int32_t)freq_error) / AFC_SLOPE); - printd("afc_correct(error=%dHz, arfcn=%u): delta=%d, afc_dac(old=%d,new=%d)\n", + //MTZ + //printd("afc_correct(error=%dHz, arfcn=%u): delta=%d, afc_dac(old=%d,new=%d)\n", + // freq_error, arfcn, delta, afc_state.dac_value, afc_state.dac_value+delta); + printf("afc_correct(error=%dHz, arfcn=%u): delta=%d, afc_dac(old=%d,new=%d)\n", freq_error, arfcn, delta, afc_state.dac_value, afc_state.dac_value+delta); afc_state.dac_value += delta; diff --git a/src/target/firmware/layer1/l23_api.c b/src/target/firmware/layer1/l23_api.c index daffaf8b..28c900a6 100644 --- a/src/target/firmware/layer1/l23_api.c +++ b/src/target/firmware/layer1/l23_api.c @@ -236,6 +236,10 @@ static void l1ctl_rx_dm_est_req(struct msgb *msg) struct l1ctl_info_ul *ul = (struct l1ctl_info_ul *) l1h->data; struct l1ctl_dm_est_req *est_req = (struct l1ctl_dm_est_req *) ul->payload; + //MTZ - added + l1s.new_dm = 1; + //l1s.ho_arfcn = 0; + printd("L1CTL_DM_EST_REQ (arfcn=%u, chan_nr=0x%02x, tsc=%u)\n", ntohs(est_req->h0.band_arfcn), ul->chan_nr, est_req->tsc); @@ -243,10 +247,11 @@ static void l1ctl_rx_dm_est_req(struct msgb *msg) mframe_disable(MF_TASK_NEIGH_PM51_C0T0); /* configure dedicated channel state */ - l1s.dedicated.type = chan_nr2dchan_type(ul->chan_nr); - l1s.dedicated.tsc = est_req->tsc; - l1s.dedicated.tn = ul->chan_nr & 0x7; - l1s.dedicated.h = est_req->h; + l1s.dedicated.chan_nr = ul->chan_nr; + l1s.dedicated.type = chan_nr2dchan_type(ul->chan_nr); + l1s.dedicated.tsc = est_req->tsc; + l1s.dedicated.tn = ul->chan_nr & 0x7; + l1s.dedicated.h = est_req->h; if (est_req->h) { int i; @@ -271,8 +276,15 @@ static void l1ctl_rx_dm_est_req(struct msgb *msg) /* Audio path */ audio_set_enabled(est_req->tch_mode, est_req->audio_mode); } + + /* Handover config */ + if ((est_req->flags & L1CTL_EST_F_RXONLY)) + l1s.dedicated.rx_only = 1; + else + l1s.dedicated.rx_only = 0; /* figure out which MF tasks to enable */ + l1s.neigh_pm.n = 0; //MTZ - Uncomment l1a_mftask_set(chan_nr2mf_task_mask(ul->chan_nr, NEIGH_MODE_PM)); } @@ -337,7 +349,7 @@ static void l1ctl_rx_dm_rel_req(struct msgb *msg) dsp_load_ciph_param(0, NULL); l1a_tch_mode_set(GSM48_CMODE_SIGN); audio_set_enabled(GSM48_CMODE_SIGN, 0); - l1s.neigh_pm.n = 0; + l1s.neigh_pm.n = 0; //MTZ - Uncomment } /* receive a L1CTL_PARAM_REQ from L23 */ @@ -352,6 +364,7 @@ static void l1ctl_rx_param_req(struct msgb *msg) l1s.ta = par_req->ta; l1s.tx_power = par_req->tx_power; + l1s.dedicated.rx_only = 0; } /* receive a L1CTL_RACH_REQ from L23 */ @@ -361,11 +374,12 @@ static void l1ctl_rx_rach_req(struct msgb *msg) struct l1ctl_info_ul *ul = (struct l1ctl_info_ul *) l1h->data; struct l1ctl_rach_req *rach_req = (struct l1ctl_rach_req *) ul->payload; - printd("L1CTL_RACH_REQ (ra=0x%02x, offset=%d combined=%d)\n", - rach_req->ra, ntohs(rach_req->offset), rach_req->combined); + //printd("L1CTL_RACH_REQ (ra=0x%02x, offset=%d combined=%d)\n", + printd("L1CTL_RACH_REQ (ra=0x%02x, offset=%d combined=%d arfcn=%d)\n", //MTZ + rach_req->ra, ntohs(rach_req->offset), rach_req->combined, rach_req->arfcn); l1a_rach_req(ntohs(rach_req->offset), rach_req->combined, - rach_req->ra); + rach_req->ra, rach_req->arfcn); //MTZ - last added } /* receive a L1CTL_DATA_REQ from L23 */ @@ -376,14 +390,16 @@ static void l1ctl_rx_data_req(struct msgb *msg) struct l1ctl_data_ind *data_ind = (struct l1ctl_data_ind *) ul->payload; struct llist_head *tx_queue; - printd("L1CTL_DATA_REQ (link_id=0x%02x)\n", ul->link_id); + //MTZ - commenting as causing issues in firmware + //printd("L1CTL_DATA_REQ (link_id=0x%02x)\n", ul->link_id); msg->l3h = data_ind->data; if (ul->link_id & 0x40) { struct gsm48_hdr *gh = (struct gsm48_hdr *)(data_ind->data + 5); if (gh->proto_discr == GSM48_PDISC_RR && gh->msg_type == GSM48_MT_RR_MEAS_REP) { - printd("updating measurement report\n"); + //MTZ - commenting as causing issues in firmware + //printd("updating measurement report\n"); l1a_meas_msgb_set(msg); return; } @@ -445,6 +461,7 @@ static void l1ctl_rx_reset_req(struct msgb *msg) l1s_reset(); l1s_reset_hw(); audio_set_enabled(GSM48_CMODE_SIGN, 0); + l1s.dedicated.type = GSM_DCHAN_NONE; l1ctl_tx_reset(L1CTL_RESET_CONF, reset_req->type); break; case L1CTL_RES_T_SCHED: @@ -510,6 +527,15 @@ static void l1ctl_tx_tch_mode_conf(uint8_t tch_mode, uint8_t audio_mode) l1_queue_for_l2(msg); } +//MTZ +/* Transmit a L1CTL_TCH_MODE_CONF */ +static void l1ctl_test(void) +{ + struct msgb *msg = l1ctl_msgb_alloc(L1CTL_TEST); + + l1_queue_for_l2(msg); +} + /* receive a L1CTL_TCH_MODE_REQ from L23 */ static void l1ctl_rx_tch_mode_req(struct msgb *msg) { @@ -541,20 +567,32 @@ static void l1ctl_rx_neigh_pm_req(struct msgb *msg) /* reset list in order to prevent race condition */ l1s.neigh_pm.n = 0; /* atomic */ - l1s.neigh_pm.second = 0; /* now reset pointer and fill list */ l1s.neigh_pm.pos = 0; + l1s.neigh_pm.valid = 0; + l1s.neigh_pm.rounds = 0; l1s.neigh_pm.running = 0; + if (pm_req->n > 64) + pm_req->n = 64; for (i = 0; i < pm_req->n; i++) { l1s.neigh_pm.band_arfcn[i] = ntohs(pm_req->band_arfcn[i]); l1s.neigh_pm.tn[i] = pm_req->tn[i]; + l1s.neigh_pm.level[i] = 0; + l1s.neigh_sb.flags_bsic[i] = 0; } + l1s.neigh_sb.count = 0; printf("L1CTL_NEIGH_PM_REQ new list with %u entries\n", pm_req->n); l1s.neigh_pm.n = pm_req->n; /* atomic */ - /* on C0 enable PM on frame 51 */ + /* + * IDLE: on C0 enable PM on frame 51 + * DEDICATED: add neighbor cell task + */ if (l1s.dedicated.type == GSM_DCHAN_NONE) mframe_enable(MF_TASK_NEIGH_PM51_C0T0); + else + l1a_mftask_set(chan_nr2mf_task_mask(l1s.dedicated.chan_nr, + NEIGH_MODE_PM)); } /* receive a L1CTL_TRAFFIC_REQ from L23 */ diff --git a/src/target/firmware/layer1/mframe_sched.c b/src/target/firmware/layer1/mframe_sched.c index 7fa38c13..b4109821 100644 --- a/src/target/firmware/layer1/mframe_sched.c +++ b/src/target/firmware/layer1/mframe_sched.c @@ -50,7 +50,9 @@ struct mframe_sched_item { #define NB_QUAD_FH_DL NB_QUAD_DL #define NB_QUAD_UL nb_sched_set_ul #define NB_QUAD_FH_UL NB_QUAD_UL -#define NEIGH_PM neigh_pm_sched_set +#define NEIGH_PM_IDLE neigh_pm_idle_sched_set +#define NEIGH_PM_TCH neigh_pm_tch_sched_set +#define NEIGH_SYNC neigh_sync_sched_set /* BCCH Normal */ static const struct mframe_sched_item mf_bcch_norm[] = { @@ -210,17 +212,17 @@ static const struct mframe_sched_item mf_sdcch4_cbch[] = { /* Measurement for MF 51 C0 */ static const struct mframe_sched_item mf_neigh_pm51_c0t0[] = { - { .sched_set = NEIGH_PM , .modulo = 51, .frame_nr = 0 }, - { .sched_set = NEIGH_PM , .modulo = 51, .frame_nr = 10 }, - { .sched_set = NEIGH_PM , .modulo = 51, .frame_nr = 20 }, - { .sched_set = NEIGH_PM , .modulo = 51, .frame_nr = 30 }, - { .sched_set = NEIGH_PM , .modulo = 51, .frame_nr = 40 }, + { .sched_set = NEIGH_PM_IDLE, .modulo = 51, .frame_nr = 0 }, + { .sched_set = NEIGH_PM_IDLE, .modulo = 51, .frame_nr = 10 }, + { .sched_set = NEIGH_PM_IDLE, .modulo = 51, .frame_nr = 20 }, + { .sched_set = NEIGH_PM_IDLE, .modulo = 51, .frame_nr = 30 }, + { .sched_set = NEIGH_PM_IDLE, .modulo = 51, .frame_nr = 40 }, { .sched_set = NULL } }; /* Measurement for MF 51 */ static const struct mframe_sched_item mf_neigh_pm51[] = { - { .sched_set = NEIGH_PM , .modulo = 51, .frame_nr = 50 }, + { .sched_set = NEIGH_PM_IDLE , .modulo = 51, .frame_nr = 50 }, { .sched_set = NULL } }; @@ -275,16 +277,20 @@ static const struct mframe_sched_item mf_tch_h_0[] = { { .sched_set = TCH, .modulo = 13, .frame_nr = 6 }, { .sched_set = TCH_D, .modulo = 13, .frame_nr = 7 }, { .sched_set = TCH, .modulo = 13, .frame_nr = 8 }, - { .sched_set = TCH_D, .modulo = 13, .frame_nr = 9 }, - { .sched_set = TCH, .modulo = 13, .frame_nr = 10 }, - { .sched_set = TCH_D, .modulo = 13, .frame_nr = 11 }, + //{ .sched_set = TCH_D, .modulo = 13, .frame_nr = 9 }, + { .sched_set = TCH_D, .modulo = 26, .frame_nr = 9 }, + //{ .sched_set = TCH, .modulo = 13, .frame_nr = 10 }, + { .sched_set = TCH, .modulo = 26, .frame_nr = 10 }, + //{ .sched_set = TCH_D, .modulo = 13, .frame_nr = 11 }, + { .sched_set = TCH_D, .modulo = 26, .frame_nr = 11 }, { .sched_set = TCH_A, .modulo = 26, .frame_nr = 12, .flags = MF_F_SACCH }, { .sched_set = NULL } }; static const struct mframe_sched_item mf_tch_h_1[] = { - { .sched_set = TCH_D, .modulo = 13, .frame_nr = 0 }, + //{ .sched_set = TCH_D, .modulo = 13, .frame_nr = 0 }, + { .sched_set = TCH_D, .modulo = 26, .frame_nr = 0 }, { .sched_set = TCH, .modulo = 13, .frame_nr = 1 }, { .sched_set = TCH_D, .modulo = 13, .frame_nr = 2 }, { .sched_set = TCH, .modulo = 13, .frame_nr = 3 }, @@ -294,20 +300,44 @@ static const struct mframe_sched_item mf_tch_h_1[] = { { .sched_set = TCH, .modulo = 13, .frame_nr = 7 }, { .sched_set = TCH_D, .modulo = 13, .frame_nr = 8 }, { .sched_set = TCH, .modulo = 13, .frame_nr = 9 }, - { .sched_set = TCH_D, .modulo = 13, .frame_nr = 10 }, - { .sched_set = TCH, .modulo = 13, .frame_nr = 11 }, + //{ .sched_set = TCH_D, .modulo = 13, .frame_nr = 10 }, + { .sched_set = TCH_D, .modulo = 26, .frame_nr = 23 }, + //{ .sched_set = TCH, .modulo = 13, .frame_nr = 11 }, + { .sched_set = TCH, .modulo = 26, .frame_nr = 24 }, { .sched_set = TCH_A, .modulo = 26, .frame_nr = 25, .flags = MF_F_SACCH }, { .sched_set = NULL } }; -/* Measurement for MF 26 */ +/* + * Measurement for MF 26 + * Note: PM will be read 2 frames, later, so we can only measure every second + * frame. + */ static const struct mframe_sched_item mf_neigh_pm26_even[] = { - { .sched_set = NEIGH_PM , .modulo = 26, .frame_nr = 25 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 0 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 2 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 4 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 6 }, + //{ .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 8 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 26, .frame_nr = 8 }, + //{ .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 10 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 26, .frame_nr = 10 }, + //{ .sched_set = NEIGH_SYNC, .modulo = 26, .frame_nr = 24 }, + { .sched_set = NEIGH_SYNC, .modulo = 26, .frame_nr = 22 }, { .sched_set = NULL } }; static const struct mframe_sched_item mf_neigh_pm26_odd[] = { - { .sched_set = NEIGH_PM , .modulo = 26, .frame_nr = 12 }, + //{ .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 0 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 26, .frame_nr = 0 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 2 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 4 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 6 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 8 }, + //{ .sched_set = NEIGH_PM_TCH, .modulo = 13, .frame_nr = 10 }, + { .sched_set = NEIGH_PM_TCH, .modulo = 26, .frame_nr = 23 }, + //{ .sched_set = NEIGH_SYNC, .modulo = 26, .frame_nr = 11 }, + { .sched_set = NEIGH_SYNC, .modulo = 26, .frame_nr = 10 }, { .sched_set = NULL } }; @@ -318,6 +348,7 @@ static const struct mframe_sched_item mf_tx_all_nb[] = { }; static const struct mframe_sched_item *sched_set_for_task[32] = { + [MF_TASK_BCCH_NORM] = mf_bcch_norm, [MF_TASK_BCCH_EXT] = mf_bcch_ext, [MF_TASK_CCCH] = mf_ccch, diff --git a/src/target/firmware/layer1/prim_fbsb.c b/src/target/firmware/layer1/prim_fbsb.c index 50acefcc..dafd06f0 100644 --- a/src/target/firmware/layer1/prim_fbsb.c +++ b/src/target/firmware/layer1/prim_fbsb.c @@ -84,6 +84,16 @@ struct l1a_fb_state { static struct l1a_fb_state fbs; static struct mon_state *last_fb = &fbs.mon; +static int sb_det = 0; //MTZ - This was added +static int fb_det = 0; //MTZ - This was added +uint32_t old_tpu_offset = 0; //MTZ - This was added +int total_sb_det = 0; + +int16_t nb_fb_toa = 0; //MTZ - This was added +uint16_t nb_fb_pm = 0; //MTZ - This was added +uint16_t nb_fb_angle0 = 0; //MTZ - This was added +uint16_t nb_fb_angle1 = 0; //MTZ - This was added +uint16_t nb_fb_snr = 0; //MTZ - This was added static void dump_mon_state(struct mon_state *fb) { @@ -141,6 +151,7 @@ static uint8_t l1s_decode_sb(struct gsm_time *time, uint32_t sb) /* TS 05.02 Chapter 4.3.3 TDMA frame number */ time->fn = gsm_gsmtime2fn(time); + printf("\n\nMTZ: time->fn = %d\n\n", time->fn); time->tc = (time->fn / 51) % 8; @@ -168,6 +179,30 @@ static void read_sb_result(struct mon_state *st, int attempt) dsp_api.r_page_used = 1; } +static void read_sb_result2(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.db_r->a_serv_demod[D_TOA]; + st->pm = dsp_api.db_r->a_serv_demod[D_PM]>>3; + st->angle = dsp_api.db_r->a_serv_demod[D_ANGLE]; + st->snr = dsp_api.db_r->a_serv_demod[D_SNR]; + + st->freq_diff = ANGLE_TO_FREQ(st->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + printf("\n\n\nMTZ: st->freq_diff = %d\n\n\n", st->freq_diff); + + //MTZ - commenting out for now + if (st->snr > AFC_SNR_THRESHOLD) + afc_input(st->freq_diff, rf_arfcn, 1); + else + afc_input(st->freq_diff, rf_arfcn, 0); + + dsp_api.r_page_used = 1; +} + /* Note: When we get the SB response, it is 2 TDMA frames after the SB * actually happened, as it is a "C W W R" task */ #define SB2_LATENCY 2 @@ -235,6 +270,7 @@ static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, synchronize_tdma(&l1s.serving_cell); /* if we have recived a SYNC burst, update our local GSM time */ + printf("\n\nMTZ: current_fn = %d, fn from SB = %d\n\n", gsm_gsmtime2fn(&l1s.current_time), fbs.mon.time.fn + SB2_LATENCY); gsm_fn2gsmtime(&l1s.current_time, fbs.mon.time.fn + SB2_LATENCY); /* compute next time from new current time */ l1s.next_time = l1s.current_time; @@ -258,6 +294,7 @@ static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, l1s_reset_hw(); /* enable the MF Task for BCCH reading */ mframe_enable(MF_TASK_BCCH_NORM); + printf("\nMTZ: l1s.serving_cell.ccch_mode = %d\n", l1s.serving_cell.ccch_mode); if (l1s.serving_cell.ccch_mode == CCCH_MODE_COMBINED) mframe_enable(MF_TASK_CCCH_COMB); else if (l1s.serving_cell.ccch_mode == CCCH_MODE_NON_COMBINED) @@ -269,6 +306,9 @@ static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, l1s_compl_sched(L1_COMPL_FB); + //MTZ - delete this + //mframe_enable(MF_TASK_TEST1); + return 0; } @@ -284,11 +324,14 @@ static int l1s_sbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, dsp_api.ndb->d_fb_mode = 0; /* wideband search */ /* Program TPU */ + printf("\nMTZ: arfcn in l1s_sbdet_cmd = %d\n", rf_arfcn); l1s_rx_win_ctrl(rf_arfcn, L1_RXWIN_SB, 0); return 0; } +static const struct tdma_sched_item sb_sched_set[]; + /* This is how it is done by the TSM30 */ static const struct tdma_sched_item sb_sched_set[] = { SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 1), SCHED_END_FRAME(), @@ -384,6 +427,7 @@ static int l1s_fbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, dsp_api.ndb->d_fb_mode = fb_mode; /* Program TPU */ + printf("\nMTZ: arfcn in l1s_fbdet_cmd = %d\n", fbs.req.band_arfcn); l1s_rx_win_ctrl(fbs.req.band_arfcn, L1_RXWIN_FB, 0); return 0; @@ -562,15 +606,601 @@ void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req) /* Reset the TOA loop counters */ toa_reset(); - if (fbs.req.flags & L1CTL_FBSB_F_FB0) - tdma_schedule_set(base_fn, fb_sched_set, 0); - else if (fbs.req.flags & L1CTL_FBSB_F_FB1) - tdma_schedule_set(base_fn, fb_sched_set, 0); - else if (fbs.req.flags & L1CTL_FBSB_F_SB) - tdma_schedule_set(base_fn, sb_sched_set, 0); + tdma_schedule_set(0, fb_sched_set, 0); + + //MTZ - Changed + //if (fbs.req.flags & L1CTL_FBSB_F_FB0) + // tdma_schedule_set(base_fn, fb_sched_set, 0); + //else if (fbs.req.flags & L1CTL_FBSB_F_FB1) + // tdma_schedule_set(base_fn, fb_sched_set, 0); + //else if (fbs.req.flags & L1CTL_FBSB_F_SB) + // tdma_schedule_set(base_fn, sb_sched_set, 0); + + //MTZ + //l1ctl_test(); + //struct msgb *msg1 = l1ctl_msgb_alloc(L1CTL_TEST); + + //l1_queue_for_l2(msg1); + +} + +/* SB for Neighbours in dedicated mode + * + * Only when number of neighbor cells is > 0, perform synchronization. + * + * For each synchronization, l1s.neigh_pm.running is set. In case of an update + * of neighbor cell list, this state is cleared, so a pending sync result would + * be ignored. + * + * After a (new) list of neighbor cells are received, the measurements are not + * yet valid. A valid state flag is used to indicate valid measurements. Until + * there are no valid measurements, the synchronization is not performed. + * + * The task is to scan the 6 strongest neighbor cells by trying to synchronize + * to it. This is done by selecting the strongest unscanned neighbor cell. + * If 6 cells have been scanned or all cells (if less than 6) have been + * scanned, the process clears all 'scanned' flags and starts over with the + * strongest (now the strongest unscanned) cell. + * + * Each synchronization attempt is performed during the "search frame" (IDLE + * frame). The process attempts to sync 11 times to ensure that it hits the + * SCH of the neighbor's BCCH. (The alignment of BCCH shifts after every two + * 26-multiframe in a way that the "search frame" is aligned with the SCH, at + * least once for 11 successive "search frames".) + * + * If the synchronization attempt is successful, the BSIC and neighbor cell + * offset is stored. These are indicated to layer23 with the measurement + * results. + * + * When performing handover to a neighbor cell, the stored offset is used to + * calculate new GSM time and tpu_offset. + */ + +static void select_neigh_cell(void) +{ + uint8_t strongest = 0, strongest_unscanned = 0; + int strongest_i = 0, strongest_unscanned_i = -1; + int num_scanned = 0; + int i; + + /* find strongest cell and strongest unscanned cell and count */ + for (i = 0; i < l1s.neigh_pm.n; i++) { + if (l1s.neigh_pm.level[i] > strongest) { + strongest = l1s.neigh_pm.level[i]; + strongest_i = i; + } + if (!(l1s.neigh_sb.flags_bsic[i] & NEIGH_PM_FLAG_SCANNED)) { + if (l1s.neigh_pm.level[i] > strongest_unscanned) { + strongest_unscanned = l1s.neigh_pm.level[i]; + strongest_unscanned_i = i; + } + } else + num_scanned++; + } + + /* no unscanned cell or we have scanned enough */ + if (strongest_unscanned_i < 0 || num_scanned >= 6) { + /* flag all cells unscanned */ + for (i = 0; i < l1s.neigh_pm.n; i++) + l1s.neigh_sb.flags_bsic[i] &= ~NEIGH_PM_FLAG_SCANNED; + /* use strongest cell to begin scanning with */ + l1s.neigh_sb.index = strongest_i; + } else { + /* use strongest unscanned cell to begin scanning with */ + l1s.neigh_sb.index = strongest_unscanned_i; + } +} + +//MTZ - The function below has been taken from synchronize_tdma in sync.c and modified for whatever seemed necessary +void synchronize_tdma2() +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/*////////////////////////////////////////////////////////////////////////////////////////////////////// + +FB/SB detection in dedicated mode + +- First of all it must be noted that 3 additional frames have been added to the original idle frame 25 or 12 depending upon the half rate type. This gives us additional time to do any required time and frequency synchronization before detecting the synchronization burst of the neighbour as well as returning to the current settings before the TCH frame that follows. This can be seen in the file firmware/layer1/mframe_sched.c. + +- The approach to detect FB and SB in dedicated mode must be kept in mind. In dedicated mode the traffic multiframe is running on the MS that consists of 26 frames with frame 25 as idle frame (in even mode). In order to read FB of neighbour we need to read its control channel that consists of 51 frames. As the control MF size is not a multiple of traffic MF size each time the traffic idle frame appears we are at a different frame number in the control MF of the neighbour. Hence the traffic idle frame coincides with a different control channel each time and actually traverses through it. By looking at the channel assignment accross the control MF it can be seen that we will coincide with a frequency burst on the control channel when we have an idle traffic frame every 10 or 11 idle frames. If we repeatedly search for FB on traffic idle frames we should be able to detect it every 10 or 11 idle frames. The SB would appear the second idle frame following the idle frame on which FB is detected not the next (this can be verified by hand). This is the approach we use. + +- It was seen in idle mode FB/SB detection that FB is detected twice, first using FB mode 0 then using FB mode 1 which seem to provide different precision on frequency correction. After each FB detection frequency correction is performed in idle mode using afc_correct. Following FB mode 1 FB detection the quarter-bit synchronization is also performed using TOA (time of arrival of frequency burst) to synch the start of frame with the internal counters by modifying l1s.tpu_offset from what I could find. It is only after these two kinds of synchronizations/corrections that we can read the bursts on the channel properly. Synchronization burst is then read which is used to get the BSIC of the BTS and the absolute frame number it is on to update our internal variables/registers with it. Additionally further frequency correction is performed. + +- The same steps need to be performed as above but this time in steps as we can only do this whenever the idle frame appears otherwise we would be in traffic mode and wouldn't have time to perform this. At each step values for frequency compensation/time correction are stored to be used in the next step. At the beginning of every idle frame set (as we have added three more idle frames) the required synchronization/correction is performed and at the end of it it is reversed to return to original settings. l1s_neigh_fbsb_sync is used to perform any synching/correction at the start of the idle frame set. + +- fb_det and sb_det are used as control variables to guide through the synchronization process as follows: + + fb_det sb_det task + + 0 0 FB detection mode 0 + 1 0 FB detection mode 1 + 0 1 Do nothing - this is the idle frame following FB detection + 0 2 SB detection + 0 3 Do nothing + 0 4 SB detection again + +fb_det goes from 0 to 1 upon FB detection in FB mode 0. Then when FB is detected in FB mode 1 sb_det becomes 1 and fb_det goes to 0. Thereafter sb_det increments upon every idle frame. This is important to keep in mind. + +* SB detection is done second time to make sure the SB isn't missed as I thought it might be in the middle of the frames. Just a cautious check. + +- Whenever FB and/or SB are detected the results are stored in the following variables to be used in case of handover + + + l1s.tpu_offsets_arfcn[ii] - The corresponding arfcn for the indices + l1s.tpu_offsets[ii] - The quarter-bit offset required for start of frame + l1s.nb_freq_diff[ii] - Frequency correction required from FB mode 0 + 1 + l1s.nb_sb_freq_diff[ii] - Additional freq correction from SB detection + l1s.nb_frame_diff[ii] - The difference in frame of serving cell and the neighbour + l1s.nb_sb_snr[ii] - The snr received (might not really be needed) + +I know I could have used the l1s.neigh_sb struct but for now that's the way it is. This is so also because the stored number of neighbour gets reset whenever we enter a new dedicated mode but that can be worked around. + +- The TOA obtained from FB detection gives the number of GSM bits from the start of the command to detect FB till the actual detection. As it can span more than 1 frame especially in idle mode it is broken down to ntdma and qbits. ntdma denotes the complete frames and qbits the number of quarter-bits following an integer number of frames (the remainder bits x 4). This is the actual difference from the frame start of the serving cell to the frame start of the neighbour cell. + +- This schedule is activated using the multiframe scheduler whenever the idle frame comes in dedicated mode. select_neigh_cell() is used to select the next neighbour once SB of one neighbour is obtained or 15 tries to detect FB have failed (or SB is not detected following FB detection). + +- l1s.neigh_sb is the struct used to keep track of neigbour bsic/power measurements etc. This can be enhanced to store the values menioned above as well. + +- l1s.tpu_offset is the variable storing the quarter-bit offset to the start of frame for the cell to be read/transmitted to. afc_correct(...) is used to do frequency compensation. + +- Things could have been coded in a better way but that's the way it is for now. Perhaps someone else can restructure the whole thing and come up with a better approach. + +///////////////////////////////////////////////////////////////////////////////////////////////////// */ + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_sync(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + if (fb_det == 1) { + //printf("afc_correct in l1s_neigh_fbsb_sync for FB1 - nb_fb_angle0\n\n"); + afc_correct(ANGLE_TO_FREQ(nb_fb_angle0), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + } + + if ((sb_det == 2)||(sb_det == 4)) { + //printf("\nMTZ - in l1s_neigh_fbsb_sync, old_tpu_offset = %d\n", l1s.tpu_offset); + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; //MTZ - uncomment + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; //MTZ - uncomment + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + //printf("MTZ - old_tpu_offset = %d, tpu_shift = %d, qbits = %d\n", old_tpu_offset, tpu_shift, qbits); + + l1s.neigh_pm.tpu_offset[l1s.neigh_sb.index] = tpu_shift; + + int ii =0; + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]) { + l1s.tpu_offsets[ii] = tpu_shift; + l1s.nb_freq_diff[ii] = ANGLE_TO_FREQ(nb_fb_angle0)+ANGLE_TO_FREQ(nb_fb_angle1); + break; + } + if (l1s.tpu_offsets_arfcn[ii] == 0) { + l1s.tpu_offsets_arfcn[ii] = l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]; + l1s.tpu_offsets[ii] = tpu_shift; + l1s.nb_freq_diff[ii] = ANGLE_TO_FREQ(nb_fb_angle0)+ANGLE_TO_FREQ(nb_fb_angle1); + break; + } + } + + //printf("\n\nMTZ: Stored TPU Offsets, Angles:"); + //for (ii=0; ii<64; ii++) { + // if (l1s.tpu_offsets_arfcn[ii] == 0) + // break; + // printf(" %d,%d(%d)", l1s.tpu_offsets[ii], l1s.nb_freq_diff[ii], l1s.tpu_offsets_arfcn[ii]); + //} + //printf("\n\n"); + + //MTZ - possibly remove the >=50 if statement + if (nb_fb_toa >= 50) { + l1s.tpu_offset = tpu_shift; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(tpu_shift); + } + //printf("afc_correct in l1s_neigh_fbsb_sync for SB - nb_fb_angle0+nb_fb_angle1\n\n"); + afc_correct(ANGLE_TO_FREQ(nb_fb_angle0)+ANGLE_TO_FREQ(nb_fb_angle1), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + } + +} +/* scheduler callback to issue a FB and SB detection task to the DSP in dedicated mode */ +// READ COMMENTS ABOVE l1s_neigh_fbsb_sync +static int l1s_neigh_fbsb_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + index = l1s.neigh_sb.index; + } + +// //MTZ - putting this for now as we wanted to repeatedly detect the remaining ones - remove +// while (!((l1s.neigh_sb.flags_bsic[index] & NEIGH_PM_FLAG_BSIC) == 0)) { +//// printf("\nMTZ: BSIC has been decoded for ARFCN %d (flags_bsic[%d] = %d)\n\n", l1s.neigh_pm.band_arfcn[index], index, l1s.neigh_sb.flags_bsic[index]); +// l1s.neigh_sb.count = 0; +// l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; +// select_neigh_cell(); +// index = l1s.neigh_sb.index; +// } + + if (sb_det == 0) { + + //l1s.fb.mode = fb_mode; + + //printf(" - detect FB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_FB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + if (fb_det == 1) { + dsp_api.ndb->d_fb_mode = 1; + } else { + dsp_api.ndb->d_fb_mode = 0; + } + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 5); //MTZ - Original - don't think works - as we have multiple idle frames now we can use TS 0 + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + } else if ((sb_det == 2)||(sb_det == 4)) { + +// printf(" - detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + //MTZ Changed + //dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ + dsp_api.db_w->d_task_md = SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + +// //MTZ - Experimenting +// dsp_api.ndb->a_sync_demod[D_TOA] = nb_fb_toa; +// dsp_api.ndb->a_sync_demod[D_PM] = nb_fb_pm; +// dsp_api.ndb->a_sync_demod[D_ANGLE] = nb_fb_angle; +// dsp_api.ndb->a_sync_demod[D_SNR] = nb_fb_snr; + + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); //MTZ - Original - don't think works - as we have multiple idle frames now we can use TS 0 + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + } + return 0; +} + +/* scheduler callback to issue a FB and SB detection task to the DSP in dedicate mode */ +// READ COMMENTS ABOVE l1s_neigh_fbsb_sync +static int l1s_neigh_fbsb_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + uint8_t bsic; + int sb_found = 0; + + if (sb_det == 0) { + if (fb_det == 1) { + //printf("afc_correct (-ve) in l1s_neigh_fbsb_resp for FB1 - nb_fb_angle0\n\n"); + afc_correct(-1*ANGLE_TO_FREQ(nb_fb_angle0), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + } + if (!dsp_api.ndb->d_fb_det) { + printf("MTZ: ARFCN %d (index %d, power %d dbm, try #%d) FB%d found = 0\n", l1s.neigh_pm.band_arfcn[index], index, rxlev2dbm(l1s.neigh_pm.level[index]), l1s.neigh_sb.count, fb_det); + + /* next sync */ + if (++l1s.neigh_sb.count == 15) { + //MTZ - a count of 0 will result in select_neigh_cell() being called and next cell being selected + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + //MTZ - If 15 tries in FB mode 1 then set fb_det to 0 + if (fb_det == 1){ + fb_det = 0; + } + } + + } else { + //MTZ - Capturing the readings from FB detection - these are stored in arrays upon SB detection for use in case handover is required + nb_fb_toa = dsp_api.ndb->a_sync_demod[D_TOA]; + nb_fb_pm = dsp_api.ndb->a_sync_demod[D_PM]; + if (fb_det == 1) + nb_fb_angle1 = dsp_api.ndb->a_sync_demod[D_ANGLE]; + else + nb_fb_angle0 = dsp_api.ndb->a_sync_demod[D_ANGLE]; + nb_fb_snr = dsp_api.ndb->a_sync_demod[D_SNR]; + printf("\n\nMTZ: ARFCN %d (index %d, power %d dbm, try #%d) FB%d found = 1 >>> nb_fb_toa = %d, angle = %d\n\n", l1s.neigh_pm.band_arfcn[index], index, rxlev2dbm(l1s.neigh_pm.level[index]), l1s.neigh_sb.count, fb_det, nb_fb_toa, dsp_api.ndb->a_sync_demod[D_ANGLE]); + //MTZ - If FB mode was 0 make it 1, it FB mode was one set sb_det to 1 to indicate SB detection step + if (fb_det == 0) { + fb_det = 1; + l1s.neigh_sb.count = 1; + } else { + sb_det = 1; + fb_det = 0; + } + } + + //l1s_reset_hw(); + tdma_sched_reset(); + } else { + if ((sb_det == 2)||(sb_det == 4)) { + /* check if sync was successful */ + + //MTZ - This was the main change below - we need to read a_sch26 as opposed to a_sch + //if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + if (dsp_api.ndb->a_sch26[0] & (1<<B_SCH_CRC)) { +// printf("\nSB found = 0 (ARFCN %d, power %d dbm)\n\n", l1s.neigh_pm.band_arfcn[index], rxlev2dbm(l1s.neigh_pm.level[index])); + } else { + + uint32_t fn; /* FN count */ + uint16_t t1; /* FN div (26*51) */ + uint8_t t2; /* FN modulo 26 */ + uint8_t t3; /* FN modulo 51 */ + uint8_t tc; + uint8_t t3p; + + sb_found = 1; + sb = dsp_api.ndb->a_sch26[3] | dsp_api.ndb->a_sch26[4] << 16; + bsic = (sb >> 2) & 0x3f; + + t1 = ((sb >> 23) & 1) | ((sb >> 7) & 0x1fe) | ((sb << 9) & 0x600); + t2 = (sb >> 18) & 0x1f; + t3p = ((sb >> 24) & 1) | ((sb >> 15) & 6); + t3 = t3p*10 + 1; + + /* TS 05.02 Chapter 4.3.3 TDMA frame number */ + fn = (51 * ((t3 - t2 + 26) % 26) + t3 + (26 * 51 * t1)) + SB2_LATENCY; + + int ii =0; + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]) { + l1s.nb_frame_diff[ii] = fn - l1s.current_time.fn; + l1s.nb_sb_freq_diff[ii] = ANGLE_TO_FREQ(dsp_api.db_r->a_serv_demod[D_ANGLE]); + l1s.nb_sb_snr[ii] = dsp_api.db_r->a_serv_demod[D_SNR]; + break; + } + } + + total_sb_det++; + //printf("=> SB 0x%08"PRIx32": BSIC=%u \n\n", sb, bsic); + printf("\n----------------------------------------------------------------------------\nSB found = 1 (ARFCN %d, power %d dbm) => SB 0x%08"PRIx32": BSIC=%u, TOA=%d, Angle=%d (Total=%d)\n----------------------------------------------------------------------------\n\n", l1s.neigh_pm.band_arfcn[index], rxlev2dbm(l1s.neigh_pm.level[index]), sb, bsic, dsp_api.db_r->a_serv_demod[D_TOA], dsp_api.db_r->a_serv_demod[D_ANGLE], total_sb_det); + l1s.neigh_sb.flags_bsic[index] = bsic | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + } + + if ((sb_det == 2)||(sb_det == 4)) { + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50); + l1s.tpu_offset = old_tpu_offset; + //printf("afc_correct (-ve) in l1s_neigh_fbsb_resp for SB - nb_fb_angle0+nb_fb_angle1\n\n"); + afc_correct(-1*(ANGLE_TO_FREQ(nb_fb_angle0) + ANGLE_TO_FREQ(nb_fb_angle1)), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + + } + + if ((sb_det == 4)||(sb_found == 1)) { + l1s.neigh_sb.count = 0; + //MTZ - need to change this statement based on detection + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + + l1s.neigh_sb.running = 0; + + //dsp_api.r_page_used = 1; + + if (sb_found == 0) + printf("\n\n"); + } + + } + + if ((sb_det == 4)||(sb_found == 1)) + sb_det = 0; + else + sb_det++; + } + + return 0; +} + +//MTZ - THIS FUNCTION BELOW IS NOT USED!!!!! +static int l1s_neigh_sb_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\n\n\nMTZ: In neigh_sb_cmd, l1s.neigh_pm.n = %d\n\n\n", l1s.neigh_pm.n); + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + return 0; } +//MTZ - THIS FUNCTION BELOW IS NOT USED!!!!! +static int l1s_neigh_sb_resp(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + + if (l1s.neigh_pm.n == 0 || !l1s.neigh_sb.running) + goto out; + + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("SB error arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + } else { + l1s.neigh_sb.count = 0; + + read_sb_result(last_fb, 1); + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + l1s.neigh_sb.flags_bsic[index] = + l1s_decode_sb(&fbs.mon.time, sb) + | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + printf("SB OK!!!!!! arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* store time offset */ + } + +out: + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + printf("\nMTZ: In l1s_neigh_sb_resp, l1s.serving_cell.arfcn = %d\n", l1s.serving_cell.arfcn); + + return 0; + +} + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(l1s_neigh_sb_cmd, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_neigh_sb_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +const struct tdma_sched_item neigh_sync_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_fbsb_sync, 1, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_neigh_fbsb_cmd, 1, 0, 1), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_fbsb_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + static __attribute__ ((constructor)) void l1s_prim_fbsb_init(void) { l1s.completion[L1_COMPL_FB] = &l1a_fb_compl; diff --git a/src/target/firmware/layer1/prim_fbsbSynchUsingDedModeDSPTasksInIdleCopy.c b/src/target/firmware/layer1/prim_fbsbSynchUsingDedModeDSPTasksInIdleCopy.c new file mode 100644 index 00000000..6139629d --- /dev/null +++ b/src/target/firmware/layer1/prim_fbsbSynchUsingDedModeDSPTasksInIdleCopy.c @@ -0,0 +1,1559 @@ +/* Layer 1 - FCCH and SCH burst handling */ + +/* (C) 2010 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <inttypes.h> +#include <defines.h> +#include <debug.h> +#include <memory.h> +#include <byteorder.h> +#include <rffe.h> +#include <osmocom/gsm/gsm_utils.h> +#include <osmocom/core/msgb.h> +#include <calypso/dsp_api.h> +#include <calypso/irq.h> +#include <calypso/tpu.h> +#include <calypso/tsp.h> +#include <calypso/dsp.h> +#include <calypso/timer.h> +#include <comm/sercomm.h> + +#include <layer1/sync.h> +#include <layer1/afc.h> +#include <layer1/toa.h> +#include <layer1/tdma_sched.h> +#include <layer1/mframe_sched.h> +#include <layer1/tpu_window.h> +#include <layer1/l23_api.h> +#include <layer1/agc.h> + +#include <l1ctl_proto.h> + +#define FB0_RETRY_COUNT 3 +#define AFC_RETRY_COUNT 30 + +extern uint16_t rf_arfcn; // TODO + +struct mon_state { + uint32_t fnr_report; /* frame number when DSP reported it */ + int attempt; /* which attempt was this ? */ + + int16_t toa; + uint16_t pm; + uint16_t angle; + uint16_t snr; + + /* computed values */ + int16_t freq_diff; + + /* Sync Burst (SB) */ + uint8_t bsic; + struct gsm_time time; +}; + +struct l1a_fb_state { + struct mon_state mon; + struct l1ctl_fbsb_req req; + int16_t initial_freq_err; + uint8_t fb_retries; + uint8_t afc_retries; +}; + +static struct l1a_fb_state fbs; +static struct mon_state *last_fb = &fbs.mon; +static int sb_det = 0; //MTZ - This was added +static int fb_det = 0; //MTZ - This was added +uint32_t old_tpu_offset = 0; //MTZ - This was added +int total_sb_det = 0; +int attempt2 = 0; + +int16_t nb_fb_toa = 0; //MTZ - This was added +uint16_t nb_fb_pm = 0; //MTZ - This was added +uint16_t nb_fb_angle0 = 0; //MTZ - This was added +uint16_t nb_fb_angle1 = 0; //MTZ - This was added +uint16_t nb_fb_snr = 0; //MTZ - This was added + +// MTZ - for working of these variables see comments in l1s_neigh_fbsb_cmd +// MTZ - det_serving_cell overrides neigh_for_fbsb_det +int synchronize_yes = 0; //MTZ - A test variable +int det_serving_cell = 0; //MTZ - A test variable +int neigh_for_fbsb_det = 0; //MTZ - A test variable + +static void dump_mon_state(struct mon_state *fb) +{ +#if 0 + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz, " + "SNR=%04x(%d.%u) OFFSET=%u SYNCHRO=%u\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle), + fb->snr, l1s_snr_int(fb->snr), l1s_snr_fract(fb->snr), + tpu_get_offset(), tpu_get_synchro()); +#else + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle)); +#endif +} + +static int l1ctl_fbsb_resp(uint8_t res) +{ + struct msgb *msg; + struct l1ctl_fbsb_conf *resp; + + msg = l1_create_l2_msg(L1CTL_FBSB_CONF, fbs.mon.time.fn, + l1s_snr_int(fbs.mon.snr), + fbs.req.band_arfcn); + if (!msg) + return -ENOMEM; + + resp = (struct l1ctl_fbsb_conf *) msgb_put(msg, sizeof(*resp)); + resp->initial_freq_err = htons(fbs.initial_freq_err); + resp->result = res; + resp->bsic = fbs.mon.bsic; + + /* no need to set BSIC, as it is never used here */ + l1_queue_for_l2(msg); + + return 0; +} + +/* SCH Burst Detection ********************************************************/ + +/* determine the GSM time and BSIC from a Sync Burst */ +static uint8_t l1s_decode_sb(struct gsm_time *time, uint32_t sb) +{ + uint8_t bsic = (sb >> 2) & 0x3f; + uint8_t t3p; + + memset(time, 0, sizeof(*time)); + + /* TS 05.02 Chapter 3.3.2.2.1 SCH Frame Numbers */ + time->t1 = ((sb >> 23) & 1) | ((sb >> 7) & 0x1fe) | ((sb << 9) & 0x600); + time->t2 = (sb >> 18) & 0x1f; + t3p = ((sb >> 24) & 1) | ((sb >> 15) & 6); + time->t3 = t3p*10 + 1; + + /* TS 05.02 Chapter 4.3.3 TDMA frame number */ + time->fn = gsm_gsmtime2fn(time); + printf("\n\nMTZ: time->fn = %d\n\n", time->fn); + + time->tc = (time->fn / 51) % 8; + + return bsic; +} + +static void read_sb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.db_r->a_serv_demod[D_TOA]; + st->pm = dsp_api.db_r->a_serv_demod[D_PM]>>3; + st->angle = dsp_api.db_r->a_serv_demod[D_ANGLE]; + st->snr = dsp_api.db_r->a_serv_demod[D_SNR]; + + st->freq_diff = ANGLE_TO_FREQ(st->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + if (st->snr > AFC_SNR_THRESHOLD) + afc_input(st->freq_diff, rf_arfcn, 1); + else + afc_input(st->freq_diff, rf_arfcn, 0); + + dsp_api.r_page_used = 1; +} + +static void read_sb_result2(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.db_r->a_serv_demod[D_TOA]; + st->pm = dsp_api.db_r->a_serv_demod[D_PM]>>3; + st->angle = dsp_api.db_r->a_serv_demod[D_ANGLE]; + st->snr = dsp_api.db_r->a_serv_demod[D_SNR]; + + st->freq_diff = ANGLE_TO_FREQ(st->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + //MTZ - commenting out for now + //if (st->snr > AFC_SNR_THRESHOLD) + // afc_input(st->freq_diff, rf_arfcn, 1); + //else + // afc_input(st->freq_diff, rf_arfcn, 0); + + //dsp_api.r_page_used = 1; +} + +/* Note: When we get the SB response, it is 2 TDMA frames after the SB + * actually happened, as it is a "C W W R" task */ +#define SB2_LATENCY 2 + +static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, + __unused uint16_t p3) +{ + uint32_t sb; + int qbits, fn_offset; + struct l1_cell_info *cinfo = &l1s.serving_cell; + int fnr_delta, bits_delta; + + putchart('s'); + + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + /* mark READ page as being used */ + dsp_api.r_page_used = 1; + + /* after 2nd attempt, we failed */ + if (attempt == 2) { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + /* after 1st attempt, we simply wait for 2nd */ + return 0; + } + + printf("SB%d ", attempt); + read_sb_result(last_fb, attempt); + + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + fbs.mon.bsic = l1s_decode_sb(&fbs.mon.time, sb); + printf("=> SB 0x%08"PRIx32": BSIC=%u ", sb, fbs.mon.bsic); + l1s_time_dump(&fbs.mon.time); + + l1s.serving_cell.bsic = fbs.mon.bsic; + + /* calculate synchronisation value (TODO: only complete for qbits) */ + last_fb->toa -= 23; + qbits = last_fb->toa * 4; + fn_offset = l1s.current_time.fn; // TODO + + if (qbits > QBITS_PER_TDMA) { + qbits -= QBITS_PER_TDMA; + fn_offset -= 1; + } else if (qbits < 0) { + qbits += QBITS_PER_TDMA; + fn_offset += 1; + } + + fnr_delta = last_fb->fnr_report - attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports SB in bit that is %d bits in the " + "future?!?\n", last_fb->toa - bits_delta); + else + printf(" qbits=%u\n", qbits); + + synchronize_tdma(&l1s.serving_cell); + + /* if we have recived a SYNC burst, update our local GSM time */ + printf("\n\nMTZ: current_fn = %d, fn from SB = %d\n\n", gsm_gsmtime2fn(&l1s.current_time), fbs.mon.time.fn + SB2_LATENCY); + gsm_fn2gsmtime(&l1s.current_time, fbs.mon.time.fn + SB2_LATENCY); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + /* If we call tdma_sched_reset(), which is only needed if there + * are further l1s_sbdet_resp() scheduled, we will bring + * dsp_api.db_r and dsp_api.db_w out of sync because we changed + * dsp_api.db_w for l1s_sbdet_cmd() and canceled + * l1s_sbdet_resp() which would change dsp_api.db_r. The DSP + * however expects dsp_api.db_w and dsp_api.db_r to be in sync + * (either "0 - 0" or "1 - 1"). So we have to bring dsp_api.db_w + * and dsp_api.db_r into sync again, otherwise NB reading will + * complain. We probably don't need the Abort command and could + * just bring dsp_api.db_w and dsp_api.db_r into sync. */ + if (attempt != 2) { + tdma_sched_reset(); + l1s_dsp_abort(); + } + + l1s_reset_hw(); + /* enable the MF Task for BCCH reading */ + mframe_enable(MF_TASK_BCCH_NORM); + printf("\nMTZ: l1s.serving_cell.ccch_mode = %d\n", l1s.serving_cell.ccch_mode); + if (l1s.serving_cell.ccch_mode == CCCH_MODE_COMBINED) + mframe_enable(MF_TASK_CCCH_COMB); + else if (l1s.serving_cell.ccch_mode == CCCH_MODE_NON_COMBINED) + mframe_enable(MF_TASK_CCCH); + + l1s_compl_sched(L1_COMPL_FB); + + return 0; +} + +static int l1s_sbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + putchart('S'); + + fbs.mon.bsic = 0; + fbs.mon.time.fn = 0; + + dsp_api.db_w->d_task_md = SB_DSP_TASK; + dsp_api.ndb->d_fb_mode = 0; /* wideband search */ + + /* Program TPU */ + printf("\nMTZ: arfcn in l1s_sbdet_cmd = %d\n", rf_arfcn); + l1s_rx_win_ctrl(rf_arfcn, L1_RXWIN_SB, 0); + + return 0; +} + +static int l1s_sbdet_resp2(__unused uint8_t p1, uint8_t attempt, + __unused uint16_t p3) +{ + uint32_t sb; + int qbits, fn_offset; + struct l1_cell_info *cinfo = &l1s.serving_cell; + int fnr_delta, bits_delta; + + putchart('s'); + + if (dsp_api.ndb->a_sch26[0] & (1<<B_SCH_CRC)) { + /* mark READ page as being used */ + //dsp_api.r_page_used = 1; + + /* after 2nd attempt, we failed */ + if (attempt2 == 15) { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + attempt2++; + + tdma_schedule_set(0, sb_sched_set, 0); + + /* after 1st attempt, we simply wait for 2nd */ + return 0; + } + attempt2=0; + + printf("SB%d ", attempt); + read_sb_result2(last_fb, attempt); + + sb = dsp_api.ndb->a_sch26[3] | dsp_api.ndb->a_sch26[4] << 16; + fbs.mon.bsic = l1s_decode_sb(&fbs.mon.time, sb); + printf("=> SB 0x%08"PRIx32": BSIC=%u ", sb, fbs.mon.bsic); + l1s_time_dump(&fbs.mon.time); + + l1s.serving_cell.bsic = fbs.mon.bsic; + + /* calculate synchronisation value (TODO: only complete for qbits) */ + last_fb->toa -= 23; + qbits = last_fb->toa * 4; + fn_offset = l1s.current_time.fn; // TODO + + if (qbits > QBITS_PER_TDMA) { + qbits -= QBITS_PER_TDMA; + fn_offset -= 1; + } else if (qbits < 0) { + qbits += QBITS_PER_TDMA; + fn_offset += 1; + } + + fnr_delta = last_fb->fnr_report - attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports SB in bit that is %d bits in the " + "future?!?\n", last_fb->toa - bits_delta); + else + printf(" qbits=%u\n", qbits); + + synchronize_tdma(&l1s.serving_cell); + + /* if we have recived a SYNC burst, update our local GSM time */ + printf("\n\nMTZ: current_fn = %d, fn from SB = %d\n\n", gsm_gsmtime2fn(&l1s.current_time), fbs.mon.time.fn + SB2_LATENCY); + gsm_fn2gsmtime(&l1s.current_time, fbs.mon.time.fn + SB2_LATENCY); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + /* If we call tdma_sched_reset(), which is only needed if there + * are further l1s_sbdet_resp() scheduled, we will bring + * dsp_api.db_r and dsp_api.db_w out of sync because we changed + * dsp_api.db_w for l1s_sbdet_cmd() and canceled + * l1s_sbdet_resp() which would change dsp_api.db_r. The DSP + * however expects dsp_api.db_w and dsp_api.db_r to be in sync + * (either "0 - 0" or "1 - 1"). So we have to bring dsp_api.db_w + * and dsp_api.db_r into sync again, otherwise NB reading will + * complain. We probably don't need the Abort command and could + * just bring dsp_api.db_w and dsp_api.db_r into sync. */ + if (attempt != 2) { + tdma_sched_reset(); + l1s_dsp_abort(); + } + + l1s_reset_hw(); + /* enable the MF Task for BCCH reading */ + mframe_enable(MF_TASK_BCCH_NORM); + printf("\nMTZ: l1s.serving_cell.ccch_mode = %d\n", l1s.serving_cell.ccch_mode); + if (l1s.serving_cell.ccch_mode == CCCH_MODE_COMBINED) + mframe_enable(MF_TASK_CCCH_COMB); + else if (l1s.serving_cell.ccch_mode == CCCH_MODE_NON_COMBINED) + mframe_enable(MF_TASK_CCCH); + + l1s_compl_sched(L1_COMPL_FB); + + return 0; +} + +static int l1s_sbdet_cmd2(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + putchart('S'); + + fbs.mon.bsic = 0; + fbs.mon.time.fn = 0; + + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; + dsp_api.ndb->d_fb_mode = 0; /* wideband search */ + + /* Program TPU */ + printf("\nMTZ: arfcn in l1s_sbdet_cmd2 = %d\n", rf_arfcn); + l1s_rx_win_ctrl(rf_arfcn, L1_RXWIN_SB26, 0); + + return 0; +} + +///* This is how it is done by the TSM30 */ +//static const struct tdma_sched_item sb_sched_set[] = { +// SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 1), SCHED_END_FRAME(), +// SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 2), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_sbdet_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_sbdet_resp, -4, 0, 2), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* This is how it is done by the TSM30 */ +static const struct tdma_sched_item sb_sched_set[] = { + SCHED_ITEM_DT(l1s_sbdet_cmd2, 0, 0, 1), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp2, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +void l1s_sb_test(uint8_t base_fn) +{ + tdma_schedule_set(base_fn, sb_sched_set, 0); +} +/* FCCH Burst *****************************************************************/ + +static int read_fb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.ndb->a_sync_demod[D_TOA]; + st->pm = dsp_api.ndb->a_sync_demod[D_PM]>>3; + st->angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + st->snr = dsp_api.ndb->a_sync_demod[D_SNR]; + + //last_fb->angle = clip_int16(last_fb->angle, AFC_MAX_ANGLE); + st->freq_diff = ANGLE_TO_FREQ(last_fb->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + dsp_api.ndb->d_fb_det = 0; + dsp_api.ndb->a_sync_demod[D_TOA] = 0; /* TSM30 does it (really needed ?) */ + + /* Update AFC with current frequency offset */ + afc_correct(st->freq_diff, rf_arfcn); + + //tpu_dsp_frameirq_enable(); + return 1; +} + +static void fbinfo2cellinfo(struct l1_cell_info *cinfo, + const struct mon_state *mon) +{ + int ntdma, qbits, fn_offset, fnr_delta, bits_delta; + + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + fnr_delta = last_fb->fnr_report - last_fb->attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports FB in bit that is %d bits in " + "the future?!?\n", last_fb->toa - bits_delta); + else { + int fb_fnr = (last_fb->fnr_report - last_fb->attempt) + + last_fb->toa/BITS_PER_TDMA; + printf("=>FB @ FNR %u fn_offset=%d qbits=%u\n", + fb_fnr, fn_offset, qbits); + } +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_fbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + if (fb_mode == 0) { + putchart('F'); + } else { + putchart('V'); + } + + l1s.fb.mode = fb_mode; + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = FB_DSP_TASK; /* maybe with I/Q swap? */ + dsp_api.ndb->d_fb_mode = fb_mode; + + /* Program TPU */ + printf("\nMTZ: arfcn in l1s_fbdet_cmd = %d\n", fbs.req.band_arfcn); + l1s_rx_win_ctrl(fbs.req.band_arfcn, L1_RXWIN_FB, 0); + + return 0; +} + +#if 0 +#define FB0_SNR_THRESH 2000 +#define FB1_SNR_THRESH 3000 +#else +#define FB0_SNR_THRESH 0 +#define FB1_SNR_THRESH 0 +#endif + +static const struct tdma_sched_item fb_sched_set[]; + +/* scheduler callback to check for a FB detection response */ +static int l1s_fbdet_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + putchart('f'); + + if (!dsp_api.ndb->d_fb_det) { + /* we did not detect a FB */ + + /* attempt < 12, do nothing */ + if (attempt < 12) + return 0; + + /* attempt >= 12, we simply don't find one */ + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + if (fbs.fb_retries < FB0_RETRY_COUNT) { + /* retry once more */ + tdma_schedule_set(1, fb_sched_set, 0); + fbs.fb_retries++; + } else { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; + } + + /* We found a frequency burst, reset everything */ + l1s_reset_hw(); + + printf("FB%u ", dsp_api.ndb->d_fb_mode); + read_fb_result(last_fb, attempt); + + /* if this is the first success, save freq err */ + if (!fbs.initial_freq_err) + fbs.initial_freq_err = last_fb->freq_diff; + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + /* Immediately schedule further TDMA tasklets, if requested. Doing + * this directly from L1S means we can do this quickly without any + * additional delays */ + if (fb_mode == 0) { + if (fbs.req.flags & L1CTL_FBSB_F_FB1) { + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + /* FIXME: don't only use the last but an average */ + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh1 && + last_fb->snr > FB0_SNR_THRESH) { + /* continue with FB1 task in DSP */ + tdma_schedule_set(1, fb_sched_set, 1); + } else { + if (fbs.afc_retries < AFC_RETRY_COUNT) { + tdma_schedule_set(1, fb_sched_set, 0); + fbs.afc_retries++; + } else { + /* Abort */ + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + } + } else + l1s_compl_sched(L1_COMPL_FB); + } else if (fb_mode == 1) { + if (fbs.req.flags & L1CTL_FBSB_F_SB) { + + int ntdma, qbits; + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + + int fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + int delay = fn_offset + 11 - l1s.current_time.fn - 1; + printf(" fn_offset=%d (fn=%"PRIu32" + attempt=%u + ntdma = %d)\n", + fn_offset, l1s.current_time.fn, last_fb->attempt, ntdma); + printf(" delay=%d (fn_offset=%d + 11 - fn=%"PRIu32" - 1\n", delay, + fn_offset, l1s.current_time.fn); + printf(" scheduling next FB/SB detection task with delay %u\n", delay); + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh2 && + last_fb->snr > FB1_SNR_THRESH) { + /* synchronize before reading SB */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + synchronize_tdma(&l1s.serving_cell); + tdma_schedule_set(delay, sb_sched_set, 0); + } else + tdma_schedule_set(delay, fb_sched_set, 1); + } else + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_fbdet_cmd2(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + if (fb_mode == 0) { + putchart('F'); + } else { + putchart('V'); + } + + l1s.fb.mode = fb_det2; + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_FB_DSP_TASK; /* maybe with I/Q swap? */ + dsp_api.ndb->d_fb_mode = fb_det2; + + /* Program TPU */ + printf("\nMTZ: arfcn in l1s_fbdet_cmd2 = %d\n", fbs.req.band_arfcn); + l1s_rx_win_ctrl(fbs.req.band_arfcn, L1_RXWIN_FB26, 0); + + return 0; +} + +/* scheduler callback to check for a FB detection response */ +static int l1s_fbdet_resp2(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + putchart('f'); + + if (!dsp_api.ndb->d_fb_det) { + /* we did not detect a FB */ + + /* attempt < 12, do nothing */ + if (attempt2 < 30) + tdma_schedule_set(0, fb_sched_set, 0); + + attempt2++; + + /* attempt >= 12, we simply don't find one */ + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + + return 0; + } + + attempt2 = 0; + + /* We found a frequency burst, reset everything */ + l1s_reset_hw(); + + printf("FB%u ", dsp_api.ndb->d_fb_mode); + read_fb_result(last_fb, attempt); + + /* if this is the first success, save freq err */ + if (!fbs.initial_freq_err) + fbs.initial_freq_err = last_fb->freq_diff; + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + /* Immediately schedule further TDMA tasklets, if requested. Doing + * this directly from L1S means we can do this quickly without any + * additional delays */ + if (fb_det2 == 0) { + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + /* FIXME: don't only use the last but an average */ + //if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh1 && + // last_fb->snr > FB0_SNR_THRESH) { + /* continue with FB1 task in DSP */ + fb_det2 = 1; + tdma_schedule_set(0, fb_sched_set, 0); + //} else { + // /* Abort */ + // last_fb->attempt = 13; + // l1s_compl_sched(L1_COMPL_FB); + //} + } else if (fb_det2 == 1) { + + int ntdma, qbits; + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + //int fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + //int delay = fn_offset + 11 - l1s.current_time.fn - 1; + //printf(" fn_offset=%d (fn=%"PRIu32" + attempt=%u + ntdma = %d)\n", + // fn_offset, l1s.current_time.fn, last_fb->attempt, ntdma); + //printf(" delay=%d (fn_offset=%d + 11 - fn=%"PRIu32" - 1\n", delay, + // fn_offset, l1s.current_time.fn); + //printf(" scheduling next FB/SB detection task with delay %u\n", delay); + //if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh2 && + // last_fb->snr > FB1_SNR_THRESH) { + /* synchronize before reading SB */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + synchronize_tdma(&l1s.serving_cell); + //tdma_schedule_set(delay, sb_sched_set, 0); + tdma_schedule_set(0, sb_sched_set, 0); + //} else + // tdma_schedule_set(delay, fb_sched_set, 1); + } + + return 0; +} + +///* FB detection */ +//static const struct tdma_sched_item fb_sched_set[] = { +// SCHED_ITEM_DT(l1s_fbdet_cmd, 0, 0, 0), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 2), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 3), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 4), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 5), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 6), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 7), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 8), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 9), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 10), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 11), SCHED_END_FRAME(), +// SCHED_ITEM(l1s_fbdet_resp, -4, 0, 12), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* FB detection */ +static const struct tdma_sched_item fb_sched_set[] = { + SCHED_ITEM_DT(l1s_fbdet_cmd2, 0, 0, 0), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp2, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +/* Asynchronous completion handler for FB detection */ +static void l1a_fb_compl(__unused enum l1_compl c) +{ + if (last_fb->attempt >= 13) { + /* FB detection failed, signal this via L1CTL */ + l1ctl_fbsb_resp(255); + return; + } + + /* FIME: use l1s.neigh_cell[fbs.cinfo_idx] */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + + /* send FBSB_CONF success message via L1CTL */ + l1ctl_fbsb_resp(0); +} + +void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req) +{ + /* copy + endian convert request data */ + fbs.req.band_arfcn = ntohs(req->band_arfcn); + fbs.req.timeout = ntohs(req->timeout); + fbs.req.freq_err_thresh1 = ntohs(req->freq_err_thresh1); + fbs.req.freq_err_thresh2 = ntohs(req->freq_err_thresh2); + fbs.req.num_freqerr_avg = req->num_freqerr_avg; + fbs.req.flags = req->flags; + fbs.req.sync_info_idx = req->sync_info_idx; + fbs.req.rxlev_exp = req->rxlev_exp; + + /* clear initial frequency error */ + fbs.initial_freq_err = 0; + fbs.fb_retries = 0; + fbs.afc_retries = 0; + + /* Make sure we start at a 'center' AFCDAC output value */ + afc_reset(); + + /* Reset the TOA loop counters */ + toa_reset(); + + + fb_det2 = 0; + tdma_schedule_set(0, fb_sched_set, 0); + + //MTZ - Changed + //if (fbs.req.flags & L1CTL_FBSB_F_FB0) + // tdma_schedule_set(base_fn, fb_sched_set, 0); + //else if (fbs.req.flags & L1CTL_FBSB_F_FB1) + // tdma_schedule_set(base_fn, fb_sched_set, 0); + //else if (fbs.req.flags & L1CTL_FBSB_F_SB) + // tdma_schedule_set(base_fn, sb_sched_set, 0); + +} + +/* SB for Neighbours in dedicated mode + * + * Only when number of neighbor cells is > 0, perform synchronization. + * + * For each synchronization, l1s.neigh_pm.running is set. In case of an update + * of neighbor cell list, this state is cleared, so a pending sync result would + * be ignored. + * + * After a (new) list of neighbor cells are received, the measurements are not + * yet valid. A valid state flag is used to indicate valid measurements. Until + * there are no valid measurements, the synchronization is not performed. + * + * The task is to scan the 6 strongest neighbor cells by trying to synchronize + * to it. This is done by selecting the strongest unscanned neighbor cell. + * If 6 cells have been scanned or all cells (if less than 6) have been + * scanned, the process clears all 'scanned' flags and starts over with the + * strongest (now the strongest unscanned) cell. + * + * Each synchronization attempt is performed during the "search frame" (IDLE + * frame). The process attempts to sync 11 times to ensure that it hits the + * SCH of the neighbor's BCCH. (The alignment of BCCH shifts after every two + * 26-multiframe in a way that the "search frame" is aligned with the SCH, at + * least once for 11 successive "search frames".) + * + * If the synchronization attempt is successful, the BSIC and neighbor cell + * offset is stored. These are indicated to layer23 with the measurement + * results. + * + * When performing handover to a neighbor cell, the stored offset is used to + * calculate new GSM time and tpu_offset. + */ + +static void select_neigh_cell(void) +{ + uint8_t strongest = 0, strongest_unscanned = 0; + int strongest_i = 0, strongest_unscanned_i = -1; + int num_scanned = 0; + int i; + + /* find strongest cell and strongest unscanned cell and count */ + for (i = 0; i < l1s.neigh_pm.n; i++) { + if (l1s.neigh_pm.level[i] > strongest) { + strongest = l1s.neigh_pm.level[i]; + strongest_i = i; + } + if (!(l1s.neigh_sb.flags_bsic[i] & NEIGH_PM_FLAG_SCANNED)) { + if (l1s.neigh_pm.level[i] > strongest_unscanned) { + strongest_unscanned = l1s.neigh_pm.level[i]; + strongest_unscanned_i = i; + } + } else + num_scanned++; + } + + /* no unscanned cell or we have scanned enough */ + if (strongest_unscanned_i < 0 || num_scanned >= 6) { + /* flag all cells unscanned */ + for (i = 0; i < l1s.neigh_pm.n; i++) + l1s.neigh_sb.flags_bsic[i] &= ~NEIGH_PM_FLAG_SCANNED; + /* use strongest cell to begin scanning with */ + l1s.neigh_sb.index = strongest_i; + } else { + /* use strongest unscanned cell to begin scanning with */ + l1s.neigh_sb.index = strongest_unscanned_i; + } +} + +//MTZ - The function below has been taken from synchronize_tdma in sync.c and modified for whatever seemed necessary +void synchronize_tdma2() +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int sync_test1(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + printf("\n\nMTZ - sync_test1, old_tpu_offset = %d\n\n", l1s.tpu_offset); + + old_tpu_offset = l1s.tpu_offset; + + //l1s.tpu_offset = 3000; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(l1s.tpu_offset); + //tpu_enq_at(0); + //tpu_enq_at(0); + //l1s.tpu_offset = old_tpu_offset; + //l1s.tpu_offset = 2000; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(l1s.tpu_offset); + + //tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(3000); + //tpu_enq_at(0); + //tpu_enq_at(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(old_tpu_offset); + //tpu_enq_at(0); + + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(2000); + ////tpu_enq_at(0); + ////tpu_enq_at(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(old_tpu_offset); + + printf("\n\nMTZ - sync_test1, ending tpu_offset = %d\n\n", l1s.tpu_offset); + + +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int sync_test2(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + printf("\n\nMTZ - sync_test2\n\n"); + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_sync(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + if (fb_det == 1) { + afc_correct(ANGLE_TO_FREQ(nb_fb_angle0), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + } + + if ((sb_det == 2)||(sb_det == 4)) { + //printf("\nMTZ - in l1s_neigh_fbsb_sync, old_tpu_offset = %d\n", l1s.tpu_offset); + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; //MTZ - uncomment + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; //MTZ - uncomment + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + //printf("MTZ - old_tpu_offset = %d, tpu_shift = %d, qbits = %d\n", old_tpu_offset, tpu_shift, qbits); + + l1s.neigh_pm.tpu_offset[l1s.neigh_sb.index] = tpu_shift; + + int ii =0; + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]) { + l1s.tpu_offsets[ii] = tpu_shift; + l1s.nb_freq_diff[ii] = ANGLE_TO_FREQ(nb_fb_angle0)+ANGLE_TO_FREQ(nb_fb_angle1); + break; + } + if (l1s.tpu_offsets_arfcn[ii] == 0) { + l1s.tpu_offsets_arfcn[ii] = l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]; + l1s.tpu_offsets[ii] = tpu_shift; + l1s.nb_freq_diff[ii] = ANGLE_TO_FREQ(nb_fb_angle0)+ANGLE_TO_FREQ(nb_fb_angle1); + break; + } + } + + printf("\n\nMTZ: Stored TPU Offsets, Angles:"); + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == 0) + break; + printf(" %d,%d(%d)", l1s.tpu_offsets[ii], l1s.nb_freq_diff[ii], l1s.tpu_offsets_arfcn[ii]); + } + printf("\n\n"); + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50) { + l1s.tpu_offset = tpu_shift; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(tpu_shift); + } + afc_correct(ANGLE_TO_FREQ(nb_fb_angle1)+ANGLE_TO_FREQ(nb_fb_angle1), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + + if (synchronize_yes) { + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + +} +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + //printf("\nMTZ: In neigh_fbsb_cmd, l1s.neigh_pm.n = %d", l1s.neigh_pm.n); + + //---------------------------------------------------------------------------------------- + // + // Important points of note: + // ------------------------- + // + // Temporary variables used for testing: + // + // Right now we have three global variables defined at the beginning of this file + // for testing purposes: det_serving_cell, synchronize_yes and neigh_for_fbsb_det. + // + // det_serving_cell allows for detection of FBSB on serving cell, something which we + // thought should be simpler as the clock is already synched on that cell. + // + // synchronize_yes is a variable with which one can control whether offset and synch + // commands will be sent to the TPU. There was a thought that perhaps the DSP SB + // detection task for dedicated mode might not require synching due to which this + // test variable was created. Also, naturally, detection on the serving cell should + // not require synching but we could be wrong. + // + // neigh_for_fbsb_det is a variable to hardcode the neighbour one wants to detect + // FBSB on. Note that because more functionality is added to the code the mechanism + // for traversing through neighbours using select_neigh_cell and certain flags is + // disturbed for now. Due to this the index is hardcoded in the code below to + // whichever neighbour one wants to detect FBSB on (ordered by signal strength) + // using this variable. + // + // Progress so far: + // + // Right now we are able to detect FB in dedicated mode even for neighbours. One + // additional thing we have done to aid us is that we have added 3 more frames to + // our idle frame by taking up some TCH channels (this can be seen in mframe_sched). + // The call doesn't get dropped if synchronization is not performed except for very + // few cases which we need not worry about for now. However, when synchronization + // is performed the call gets dropped without exception. + // + // Few points where the problem could lie: + // + // 1) Perhaps we are not using the TCH_SB_DSP_TASK correctly. Perhaps some other + // locations in the API also need to be written to when issuing the command in + // addition to dsp_api.db_w->d_task_md. Perhaps we are not reading from the correct + // location when checking the response. + // + // 2) Perhaps we need to start the SB detection task some quarter-bits or timeslots + // before the actual SB appears. + // + // 3) Timing issues. This is the most likely cause as we haven't really been able + // to fully understand and implement timing (if it is required, which it seems like + // it is) in the code. If timing is required then this code is quite superficial. + // In idle mode functions such as afc_correct and procedures requiring frequency + // difference are used to before calling SB detection task which are not done here. + // + // Timing issues seems to be the most likely problem. Anyone wishing to solve the + // SB detection issue should try to focus on this problem, understand how + // synchronization is performed in idle mode and how we can do that in dedicated + // mode and be back within 4 frames as after that the traffic needs to resume. + // + //---------------------------------------------------------------------------------------- + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + index = l1s.neigh_sb.index; + } + +// //MTZ - putting this for now as we wanted to repeatedly detect the remaining ones - remove +// while (!((l1s.neigh_sb.flags_bsic[index] & NEIGH_PM_FLAG_BSIC) == 0)) { +//// printf("\nMTZ: BSIC has been decoded for ARFCN %d (flags_bsic[%d] = %d)\n\n", l1s.neigh_pm.band_arfcn[index], index, l1s.neigh_sb.flags_bsic[index]); +// l1s.neigh_sb.count = 0; +// l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; +// select_neigh_cell(); +// index = l1s.neigh_sb.index; +// } + + //MTZ - This index variable is used to hardcode the neighbour cell for now + //index = neigh_for_fbsb_det; + + if (sb_det == 0) { + + //l1s.fb.mode = fb_mode; + +// if (det_serving_cell) +// printf(" - detect FB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); +// else +// printf(" - detect FB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_FB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + if (fb_det == 1) { + dsp_api.ndb->d_fb_mode = 1; + } else { + dsp_api.ndb->d_fb_mode = 0; + } + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 5); //MTZ - Original - don't think works + printf("\nMTZ: arfcn in l1s_neigh_fbsb_cmd (FB%d) = %d\n", fb_det, l1s.neigh_pm.band_arfcn[index]); + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_FB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + } else if ((sb_det == 2)||(sb_det == 4)) { + +// if (det_serving_cell) +// printf(" - detect SB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); +// else +// printf(" - detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + //MTZ - This is a variable for the testing phase whether to send sync commands or not + //if (synchronize_yes) + // synchronize_tdma2(); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + +// //MTZ - Experimenting +// dsp_api.ndb->a_sync_demod[D_TOA] = nb_fb_toa; +// dsp_api.ndb->a_sync_demod[D_PM] = nb_fb_pm; +// dsp_api.ndb->a_sync_demod[D_ANGLE] = nb_fb_angle; +// dsp_api.ndb->a_sync_demod[D_SNR] = nb_fb_snr; + + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); //MTZ - Original - don't think works + printf("\nMTZ: arfcn in l1s_neigh_fbsb_cmd (SB) = %d\n", l1s.neigh_pm.band_arfcn[index]); + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_SB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + } + return 0; +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + uint8_t bsic; + int sb_found = 0; + + if (sb_det == 0) { + if (fb_det == 1) + afc_correct(-1*ANGLE_TO_FREQ(nb_fb_angle0), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + if (!dsp_api.ndb->d_fb_det) { + printf("MTZ: ARFCN %d (index %d, power %d dbm, try #%d) FB%d found = 0\n", l1s.neigh_pm.band_arfcn[index], index, rxlev2dbm(l1s.neigh_pm.level[index]), l1s.neigh_sb.count, fb_det); + + /* next sync */ + if (++l1s.neigh_sb.count == 15) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + if (fb_det == 1){ + fb_det = 0; + } + } + + } else { + nb_fb_toa = dsp_api.ndb->a_sync_demod[D_TOA]; + nb_fb_pm = dsp_api.ndb->a_sync_demod[D_PM]; + if (fb_det == 1) + nb_fb_angle1 = dsp_api.ndb->a_sync_demod[D_ANGLE]; + else + nb_fb_angle0 = dsp_api.ndb->a_sync_demod[D_ANGLE]; + nb_fb_snr = dsp_api.ndb->a_sync_demod[D_SNR]; + printf("\n\nMTZ: ARFCN %d (index %d, power %d dbm, try #%d) FB%d found = 1 >>> nb_fb_toa = %d, angle = %d\n\n", l1s.neigh_pm.band_arfcn[index], index, rxlev2dbm(l1s.neigh_pm.level[index]), l1s.neigh_sb.count, fb_det, nb_fb_toa, dsp_api.ndb->a_sync_demod[D_ANGLE]); + if (fb_det == 0) { + fb_det = 1; + l1s.neigh_sb.count = 1; + } else { + sb_det = 1; + fb_det = 0; + } + } + + //l1s_reset_hw(); + tdma_sched_reset(); + } else { + if ((sb_det == 2)||(sb_det == 4)) { + /* check if sync was successful */ + //if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + // printf("\nSB found1 = 0\n"); + //} else { + // printf("\nSB found1 = 1\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + //} + + if (dsp_api.ndb->a_sch26[0] & (1<<B_SCH_CRC)) { +// printf("\nSB found = 0 (ARFCN %d, power %d dbm)\n\n", l1s.neigh_pm.band_arfcn[index], rxlev2dbm(l1s.neigh_pm.level[index])); + } else { + + uint32_t fn; /* FN count */ + uint16_t t1; /* FN div (26*51) */ + uint8_t t2; /* FN modulo 26 */ + uint8_t t3; /* FN modulo 51 */ + uint8_t tc; + uint8_t t3p; + + sb_found = 1; + sb = dsp_api.ndb->a_sch26[3] | dsp_api.ndb->a_sch26[4] << 16; + bsic = (sb >> 2) & 0x3f; + + t1 = ((sb >> 23) & 1) | ((sb >> 7) & 0x1fe) | ((sb << 9) & 0x600); + t2 = (sb >> 18) & 0x1f; + t3p = ((sb >> 24) & 1) | ((sb >> 15) & 6); + t3 = t3p*10 + 1; + + /* TS 05.02 Chapter 4.3.3 TDMA frame number */ + fn = (51 * ((t3 - t2 + 26) % 26) + t3 + (26 * 51 * t1)) + SB2_LATENCY; + + int ii =0; + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]) { + l1s.nb_frame_diff[ii] = fn - l1s.current_time.fn; + break; + } + } + + + total_sb_det++; + //printf("=> SB 0x%08"PRIx32": BSIC=%u \n\n", sb, bsic); + printf("\n----------------------------------------------------------------------------\nSB found = 1 (ARFCN %d, power %d dbm) => SB 0x%08"PRIx32": BSIC=%u, TOA=%d, Angle=%d (Total=%d)\n----------------------------------------------------------------------------\n\n", l1s.neigh_pm.band_arfcn[index], rxlev2dbm(l1s.neigh_pm.level[index]), sb, bsic, dsp_api.db_r->a_serv_demod[D_TOA], dsp_api.db_r->a_serv_demod[D_ANGLE], total_sb_det); + l1s.neigh_sb.flags_bsic[index] = bsic | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + //if ((l1s.new_dm == 1) && (l1s.neigh_pm.band_arfcn[index] != 58)) { + // l1s.ho_arfcn = l1s.neigh_pm.band_arfcn[index]; + // l1s.new_dm = 0; + //} + } + + if ((sb_det == 2)||(sb_det == 4)) { + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50); + l1s.tpu_offset = old_tpu_offset; + afc_correct(-1*(ANGLE_TO_FREQ(nb_fb_angle0) + ANGLE_TO_FREQ(nb_fb_angle1)), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + + if (synchronize_yes) { + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + + if ((sb_det == 4)||(sb_found == 1)) { + l1s.neigh_sb.count = 0; + //MTZ - need to change this statement based on detection + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + + l1s.neigh_sb.running = 0; + + //dsp_api.r_page_used = 1; + + if (sb_found == 0) + printf("\n\n"); + } + + } + + if ((sb_det == 4)||(sb_found == 1)) + sb_det = 0; + else + sb_det++; + } + + return 0; +} + +static int l1s_neigh_sb_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\n\n\nMTZ: In neigh_sb_cmd, l1s.neigh_pm.n = %d\n\n\n", l1s.neigh_pm.n); + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + return 0; +} + +static int l1s_neigh_sb_resp(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + + if (l1s.neigh_pm.n == 0 || !l1s.neigh_sb.running) + goto out; + + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("SB error arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + } else { + l1s.neigh_sb.count = 0; + + read_sb_result(last_fb, 1); + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + l1s.neigh_sb.flags_bsic[index] = + l1s_decode_sb(&fbs.mon.time, sb) + | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + printf("SB OK!!!!!! arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* store time offset */ + } + +out: + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + printf("\nMTZ: In l1s_neigh_sb_resp, l1s.serving_cell.arfcn = %d\n", l1s.serving_cell.arfcn); + + return 0; + +} + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(l1s_neigh_sb_cmd, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_neigh_sb_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +const struct tdma_sched_item neigh_sync_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_fbsb_sync, 1, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_neigh_fbsb_cmd, 1, 0, 1), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_fbsb_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(sync_test1, 1, 0, 1), SCHED_END_FRAME(), +//// SCHED_END_FRAME(), +//// SCHED_END_FRAME(), +//// SCHED_ITEM_DT(sync_test2, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + + +static __attribute__ ((constructor)) void l1s_prim_fbsb_init(void) +{ + l1s.completion[L1_COMPL_FB] = &l1a_fb_compl; +} diff --git a/src/target/firmware/layer1/prim_fbsbcurr.c b/src/target/firmware/layer1/prim_fbsbcurr.c new file mode 100644 index 00000000..275db9dc --- /dev/null +++ b/src/target/firmware/layer1/prim_fbsbcurr.c @@ -0,0 +1,1223 @@ +/* Layer 1 - FCCH and SCH burst handling */ + +/* (C) 2010 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <inttypes.h> +#include <defines.h> +#include <debug.h> +#include <memory.h> +#include <byteorder.h> +#include <rffe.h> +#include <osmocom/gsm/gsm_utils.h> +#include <osmocom/core/msgb.h> +#include <calypso/dsp_api.h> +#include <calypso/irq.h> +#include <calypso/tpu.h> +#include <calypso/tsp.h> +#include <calypso/dsp.h> +#include <calypso/timer.h> +#include <comm/sercomm.h> + +#include <layer1/sync.h> +#include <layer1/afc.h> +#include <layer1/toa.h> +#include <layer1/tdma_sched.h> +#include <layer1/mframe_sched.h> +#include <layer1/tpu_window.h> +#include <layer1/l23_api.h> +#include <layer1/agc.h> + +#include <l1ctl_proto.h> + +#define FB0_RETRY_COUNT 3 +#define AFC_RETRY_COUNT 30 + +extern uint16_t rf_arfcn; // TODO + +struct mon_state { + uint32_t fnr_report; /* frame number when DSP reported it */ + int attempt; /* which attempt was this ? */ + + int16_t toa; + uint16_t pm; + uint16_t angle; + uint16_t snr; + + /* computed values */ + int16_t freq_diff; + + /* Sync Burst (SB) */ + uint8_t bsic; + struct gsm_time time; +}; + +struct l1a_fb_state { + struct mon_state mon; + struct l1ctl_fbsb_req req; + int16_t initial_freq_err; + uint8_t fb_retries; + uint8_t afc_retries; +}; + +static struct l1a_fb_state fbs; +static struct mon_state *last_fb = &fbs.mon; +static int sb_det = 0; //MTZ - This was added +uint32_t old_tpu_offset = 0; //MTZ - This was added +int total_sb_det = 0; + +int16_t nb_fb_toa = 0; //MTZ - This was added +uint16_t nb_fb_pm = 0; //MTZ - This was added +uint16_t nb_fb_angle = 0; //MTZ - This was added +uint16_t nb_fb_snr = 0; //MTZ - This was added + +// MTZ - for working of these variables see comments in l1s_neigh_fbsb_cmd +// MTZ - det_serving_cell overrides neigh_for_fbsb_det +int synchronize_yes = 0; //MTZ - A test variable +int det_serving_cell = 0; //MTZ - A test variable +int neigh_for_fbsb_det = 0; //MTZ - A test variable + +static void dump_mon_state(struct mon_state *fb) +{ +#if 0 + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz, " + "SNR=%04x(%d.%u) OFFSET=%u SYNCHRO=%u\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle), + fb->snr, l1s_snr_int(fb->snr), l1s_snr_fract(fb->snr), + tpu_get_offset(), tpu_get_synchro()); +#else + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle)); +#endif +} + +static int l1ctl_fbsb_resp(uint8_t res) +{ + struct msgb *msg; + struct l1ctl_fbsb_conf *resp; + + msg = l1_create_l2_msg(L1CTL_FBSB_CONF, fbs.mon.time.fn, + l1s_snr_int(fbs.mon.snr), + fbs.req.band_arfcn); + if (!msg) + return -ENOMEM; + + resp = (struct l1ctl_fbsb_conf *) msgb_put(msg, sizeof(*resp)); + resp->initial_freq_err = htons(fbs.initial_freq_err); + resp->result = res; + resp->bsic = fbs.mon.bsic; + + /* no need to set BSIC, as it is never used here */ + l1_queue_for_l2(msg); + + return 0; +} + +/* SCH Burst Detection ********************************************************/ + +/* determine the GSM time and BSIC from a Sync Burst */ +static uint8_t l1s_decode_sb(struct gsm_time *time, uint32_t sb) +{ + uint8_t bsic = (sb >> 2) & 0x3f; + uint8_t t3p; + + memset(time, 0, sizeof(*time)); + + /* TS 05.02 Chapter 3.3.2.2.1 SCH Frame Numbers */ + time->t1 = ((sb >> 23) & 1) | ((sb >> 7) & 0x1fe) | ((sb << 9) & 0x600); + time->t2 = (sb >> 18) & 0x1f; + t3p = ((sb >> 24) & 1) | ((sb >> 15) & 6); + time->t3 = t3p*10 + 1; + + /* TS 05.02 Chapter 4.3.3 TDMA frame number */ + time->fn = gsm_gsmtime2fn(time); + + time->tc = (time->fn / 51) % 8; + + return bsic; +} + +static void read_sb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.db_r->a_serv_demod[D_TOA]; + st->pm = dsp_api.db_r->a_serv_demod[D_PM]>>3; + st->angle = dsp_api.db_r->a_serv_demod[D_ANGLE]; + st->snr = dsp_api.db_r->a_serv_demod[D_SNR]; + + st->freq_diff = ANGLE_TO_FREQ(st->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + if (st->snr > AFC_SNR_THRESHOLD) + afc_input(st->freq_diff, rf_arfcn, 1); + else + afc_input(st->freq_diff, rf_arfcn, 0); + + dsp_api.r_page_used = 1; +} + +/* Note: When we get the SB response, it is 2 TDMA frames after the SB + * actually happened, as it is a "C W W R" task */ +#define SB2_LATENCY 2 + +static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, + __unused uint16_t p3) +{ + uint32_t sb; + int qbits, fn_offset; + struct l1_cell_info *cinfo = &l1s.serving_cell; + int fnr_delta, bits_delta; + + putchart('s'); + + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + /* mark READ page as being used */ + dsp_api.r_page_used = 1; + + /* after 2nd attempt, we failed */ + if (attempt == 2) { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + /* after 1st attempt, we simply wait for 2nd */ + return 0; + } + + printf("SB%d ", attempt); + read_sb_result(last_fb, attempt); + + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + fbs.mon.bsic = l1s_decode_sb(&fbs.mon.time, sb); + printf("=> SB 0x%08"PRIx32": BSIC=%u ", sb, fbs.mon.bsic); + l1s_time_dump(&fbs.mon.time); + + l1s.serving_cell.bsic = fbs.mon.bsic; + + /* calculate synchronisation value (TODO: only complete for qbits) */ + last_fb->toa -= 23; + qbits = last_fb->toa * 4; + fn_offset = l1s.current_time.fn; // TODO + + if (qbits > QBITS_PER_TDMA) { + qbits -= QBITS_PER_TDMA; + fn_offset -= 1; + } else if (qbits < 0) { + qbits += QBITS_PER_TDMA; + fn_offset += 1; + } + + fnr_delta = last_fb->fnr_report - attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports SB in bit that is %d bits in the " + "future?!?\n", last_fb->toa - bits_delta); + else + printf(" qbits=%u\n", qbits); + + synchronize_tdma(&l1s.serving_cell); + + /* if we have recived a SYNC burst, update our local GSM time */ + gsm_fn2gsmtime(&l1s.current_time, fbs.mon.time.fn + SB2_LATENCY); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + /* If we call tdma_sched_reset(), which is only needed if there + * are further l1s_sbdet_resp() scheduled, we will bring + * dsp_api.db_r and dsp_api.db_w out of sync because we changed + * dsp_api.db_w for l1s_sbdet_cmd() and canceled + * l1s_sbdet_resp() which would change dsp_api.db_r. The DSP + * however expects dsp_api.db_w and dsp_api.db_r to be in sync + * (either "0 - 0" or "1 - 1"). So we have to bring dsp_api.db_w + * and dsp_api.db_r into sync again, otherwise NB reading will + * complain. We probably don't need the Abort command and could + * just bring dsp_api.db_w and dsp_api.db_r into sync. */ + if (attempt != 2) { + tdma_sched_reset(); + l1s_dsp_abort(); + } + + l1s_reset_hw(); + /* enable the MF Task for BCCH reading */ + mframe_enable(MF_TASK_BCCH_NORM); + printf("\nMTZ: l1s.serving_cell.ccch_mode = %d\n", l1s.serving_cell.ccch_mode); + if (l1s.serving_cell.ccch_mode == CCCH_MODE_COMBINED) + mframe_enable(MF_TASK_CCCH_COMB); + else if (l1s.serving_cell.ccch_mode == CCCH_MODE_NON_COMBINED) + mframe_enable(MF_TASK_CCCH); + + l1s_compl_sched(L1_COMPL_FB); + + return 0; +} + +static int l1s_sbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + putchart('S'); + + fbs.mon.bsic = 0; + fbs.mon.time.fn = 0; + + dsp_api.db_w->d_task_md = SB_DSP_TASK; + dsp_api.ndb->d_fb_mode = 0; /* wideband search */ + + /* Program TPU */ + printf("\nMTZ: arfcn in l1s_sbdet_cmd = %d\n", rf_arfcn); + l1s_rx_win_ctrl(rf_arfcn, L1_RXWIN_SB, 0); + + return 0; +} + +/* This is how it is done by the TSM30 */ +static const struct tdma_sched_item sb_sched_set[] = { + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 2), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +void l1s_sb_test(uint8_t base_fn) +{ + tdma_schedule_set(base_fn, sb_sched_set, 0); +} +/* FCCH Burst *****************************************************************/ + +static int read_fb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.ndb->a_sync_demod[D_TOA]; + st->pm = dsp_api.ndb->a_sync_demod[D_PM]>>3; + st->angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + st->snr = dsp_api.ndb->a_sync_demod[D_SNR]; + + //last_fb->angle = clip_int16(last_fb->angle, AFC_MAX_ANGLE); + st->freq_diff = ANGLE_TO_FREQ(last_fb->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + dsp_api.ndb->d_fb_det = 0; + dsp_api.ndb->a_sync_demod[D_TOA] = 0; /* TSM30 does it (really needed ?) */ + + /* Update AFC with current frequency offset */ + afc_correct(st->freq_diff, rf_arfcn); + + //tpu_dsp_frameirq_enable(); + return 1; +} + +static void fbinfo2cellinfo(struct l1_cell_info *cinfo, + const struct mon_state *mon) +{ + int ntdma, qbits, fn_offset, fnr_delta, bits_delta; + + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + fnr_delta = last_fb->fnr_report - last_fb->attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports FB in bit that is %d bits in " + "the future?!?\n", last_fb->toa - bits_delta); + else { + int fb_fnr = (last_fb->fnr_report - last_fb->attempt) + + last_fb->toa/BITS_PER_TDMA; + printf("=>FB @ FNR %u fn_offset=%d qbits=%u\n", + fb_fnr, fn_offset, qbits); + } +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_fbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + if (fb_mode == 0) { + putchart('F'); + } else { + putchart('V'); + } + + l1s.fb.mode = fb_mode; + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = FB_DSP_TASK; /* maybe with I/Q swap? */ + dsp_api.ndb->d_fb_mode = fb_mode; + + /* Program TPU */ + printf("\nMTZ: arfcn in l1s_fbdet_cmd = %d\n", fbs.req.band_arfcn); + l1s_rx_win_ctrl(fbs.req.band_arfcn, L1_RXWIN_FB, 0); + + return 0; +} + +#if 0 +#define FB0_SNR_THRESH 2000 +#define FB1_SNR_THRESH 3000 +#else +#define FB0_SNR_THRESH 0 +#define FB1_SNR_THRESH 0 +#endif + +static const struct tdma_sched_item fb_sched_set[]; + +/* scheduler callback to check for a FB detection response */ +static int l1s_fbdet_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + putchart('f'); + + if (!dsp_api.ndb->d_fb_det) { + /* we did not detect a FB */ + + /* attempt < 12, do nothing */ + if (attempt < 12) + return 0; + + /* attempt >= 12, we simply don't find one */ + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + if (fbs.fb_retries < FB0_RETRY_COUNT) { + /* retry once more */ + tdma_schedule_set(1, fb_sched_set, 0); + fbs.fb_retries++; + } else { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; + } + + /* We found a frequency burst, reset everything */ + l1s_reset_hw(); + + printf("FB%u ", dsp_api.ndb->d_fb_mode); + read_fb_result(last_fb, attempt); + + /* if this is the first success, save freq err */ + if (!fbs.initial_freq_err) + fbs.initial_freq_err = last_fb->freq_diff; + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + /* Immediately schedule further TDMA tasklets, if requested. Doing + * this directly from L1S means we can do this quickly without any + * additional delays */ + if (fb_mode == 0) { + if (fbs.req.flags & L1CTL_FBSB_F_FB1) { + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + /* FIXME: don't only use the last but an average */ + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh1 && + last_fb->snr > FB0_SNR_THRESH) { + /* continue with FB1 task in DSP */ + tdma_schedule_set(1, fb_sched_set, 1); + } else { + if (fbs.afc_retries < AFC_RETRY_COUNT) { + tdma_schedule_set(1, fb_sched_set, 0); + fbs.afc_retries++; + } else { + /* Abort */ + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + } + } else + l1s_compl_sched(L1_COMPL_FB); + } else if (fb_mode == 1) { + if (fbs.req.flags & L1CTL_FBSB_F_SB) { + + int ntdma, qbits; + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + + int fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + int delay = fn_offset + 11 - l1s.current_time.fn - 1; + printf(" fn_offset=%d (fn=%"PRIu32" + attempt=%u + ntdma = %d)\n", + fn_offset, l1s.current_time.fn, last_fb->attempt, ntdma); + printf(" delay=%d (fn_offset=%d + 11 - fn=%"PRIu32" - 1\n", delay, + fn_offset, l1s.current_time.fn); + printf(" scheduling next FB/SB detection task with delay %u\n", delay); + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh2 && + last_fb->snr > FB1_SNR_THRESH) { + /* synchronize before reading SB */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + synchronize_tdma(&l1s.serving_cell); + tdma_schedule_set(delay, sb_sched_set, 0); + } else + tdma_schedule_set(delay, fb_sched_set, 1); + } else + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; +} + +/* FB detection */ +static const struct tdma_sched_item fb_sched_set[] = { + SCHED_ITEM_DT(l1s_fbdet_cmd, 0, 0, 0), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 3), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 4), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 5), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 6), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 7), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 8), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 9), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 10), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 11), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 12), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +/* Asynchronous completion handler for FB detection */ +static void l1a_fb_compl(__unused enum l1_compl c) +{ + if (last_fb->attempt >= 13) { + /* FB detection failed, signal this via L1CTL */ + l1ctl_fbsb_resp(255); + return; + } + + /* FIME: use l1s.neigh_cell[fbs.cinfo_idx] */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + + /* send FBSB_CONF success message via L1CTL */ + l1ctl_fbsb_resp(0); +} + +void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req) +{ + /* copy + endian convert request data */ + fbs.req.band_arfcn = ntohs(req->band_arfcn); + fbs.req.timeout = ntohs(req->timeout); + fbs.req.freq_err_thresh1 = ntohs(req->freq_err_thresh1); + fbs.req.freq_err_thresh2 = ntohs(req->freq_err_thresh2); + fbs.req.num_freqerr_avg = req->num_freqerr_avg; + fbs.req.flags = req->flags; + fbs.req.sync_info_idx = req->sync_info_idx; + fbs.req.rxlev_exp = req->rxlev_exp; + + /* clear initial frequency error */ + fbs.initial_freq_err = 0; + fbs.fb_retries = 0; + fbs.afc_retries = 0; + + /* Make sure we start at a 'center' AFCDAC output value */ + afc_reset(); + + /* Reset the TOA loop counters */ + toa_reset(); + + if (fbs.req.flags & L1CTL_FBSB_F_FB0) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_FB1) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_SB) + tdma_schedule_set(base_fn, sb_sched_set, 0); + +} + +/* SB for Neighbours in dedicated mode + * + * Only when number of neighbor cells is > 0, perform synchronization. + * + * For each synchronization, l1s.neigh_pm.running is set. In case of an update + * of neighbor cell list, this state is cleared, so a pending sync result would + * be ignored. + * + * After a (new) list of neighbor cells are received, the measurements are not + * yet valid. A valid state flag is used to indicate valid measurements. Until + * there are no valid measurements, the synchronization is not performed. + * + * The task is to scan the 6 strongest neighbor cells by trying to synchronize + * to it. This is done by selecting the strongest unscanned neighbor cell. + * If 6 cells have been scanned or all cells (if less than 6) have been + * scanned, the process clears all 'scanned' flags and starts over with the + * strongest (now the strongest unscanned) cell. + * + * Each synchronization attempt is performed during the "search frame" (IDLE + * frame). The process attempts to sync 11 times to ensure that it hits the + * SCH of the neighbor's BCCH. (The alignment of BCCH shifts after every two + * 26-multiframe in a way that the "search frame" is aligned with the SCH, at + * least once for 11 successive "search frames".) + * + * If the synchronization attempt is successful, the BSIC and neighbor cell + * offset is stored. These are indicated to layer23 with the measurement + * results. + * + * When performing handover to a neighbor cell, the stored offset is used to + * calculate new GSM time and tpu_offset. + */ + +static void select_neigh_cell(void) +{ + uint8_t strongest = 0, strongest_unscanned = 0; + int strongest_i = 0, strongest_unscanned_i = -1; + int num_scanned = 0; + int i; + + /* find strongest cell and strongest unscanned cell and count */ + for (i = 0; i < l1s.neigh_pm.n; i++) { + if (l1s.neigh_pm.level[i] > strongest) { + strongest = l1s.neigh_pm.level[i]; + strongest_i = i; + } + if (!(l1s.neigh_sb.flags_bsic[i] & NEIGH_PM_FLAG_SCANNED)) { + if (l1s.neigh_pm.level[i] > strongest_unscanned) { + strongest_unscanned = l1s.neigh_pm.level[i]; + strongest_unscanned_i = i; + } + } else + num_scanned++; + } + + /* no unscanned cell or we have scanned enough */ + if (strongest_unscanned_i < 0 || num_scanned >= 6) { + /* flag all cells unscanned */ + for (i = 0; i < l1s.neigh_pm.n; i++) + l1s.neigh_sb.flags_bsic[i] &= ~NEIGH_PM_FLAG_SCANNED; + /* use strongest cell to begin scanning with */ + l1s.neigh_sb.index = strongest_i; + } else { + /* use strongest unscanned cell to begin scanning with */ + l1s.neigh_sb.index = strongest_unscanned_i; + } +} + +//MTZ - The function below has been taken from synchronize_tdma in sync.c and modified for whatever seemed necessary +void synchronize_tdma2() +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int sync_test1(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + printf("\n\nMTZ - sync_test1, old_tpu_offset = %d\n\n", l1s.tpu_offset); + + old_tpu_offset = l1s.tpu_offset; + + //l1s.tpu_offset = 3000; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(l1s.tpu_offset); + //tpu_enq_at(0); + //tpu_enq_at(0); + //l1s.tpu_offset = old_tpu_offset; + //l1s.tpu_offset = 2000; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(l1s.tpu_offset); + + //tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(3000); + //tpu_enq_at(0); + //tpu_enq_at(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(old_tpu_offset); + //tpu_enq_at(0); + + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(2000); + ////tpu_enq_at(0); + ////tpu_enq_at(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(old_tpu_offset); + + printf("\n\nMTZ - sync_test1, ending tpu_offset = %d\n\n", l1s.tpu_offset); + + +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int sync_test2(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + printf("\n\nMTZ - sync_test2\n\n"); + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_sync(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + if ((sb_det == 2)||(sb_det == 4)) { + //printf("\nMTZ - in l1s_neigh_fbsb_sync, old_tpu_offset = %d\n", l1s.tpu_offset); + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; //MTZ - uncomment + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; //MTZ - uncomment + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + printf("MTZ - old_tpu_offset = %d, tpu_shift = %d, qbits = %d\n", old_tpu_offset, tpu_shift, qbits); + + l1s.neigh_pm.tpu_offset[l1s.neigh_sb.index] = tpu_shift; + + int ii =0; + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]) { + l1s.tpu_offsets[ii] = tpu_shift; + l1s.nb_freq_diff[ii] = ANGLE_TO_FREQ(nb_fb_angle); + break; + } + if (l1s.tpu_offsets_arfcn[ii] == 0) { + l1s.tpu_offsets_arfcn[ii] = l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]; + l1s.tpu_offsets[ii] = tpu_shift; + l1s.nb_freq_diff[ii] = ANGLE_TO_FREQ(nb_fb_angle); + break; + } + } + + printf("\n\nMTZ: Stored TPU Offsets, Angles:"); + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == 0) + break; + printf(" %d,%d(%d)", l1s.tpu_offsets[ii], l1s.nb_freq_diff[ii], l1s.tpu_offsets_arfcn[ii]); + } + printf("\n\n"); + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50) { + l1s.tpu_offset = tpu_shift; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(tpu_shift); + } + afc_correct(ANGLE_TO_FREQ(nb_fb_angle), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + + if (synchronize_yes) { + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + +} +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + //printf("\nMTZ: In neigh_fbsb_cmd, l1s.neigh_pm.n = %d", l1s.neigh_pm.n); + + //---------------------------------------------------------------------------------------- + // + // Important points of note: + // ------------------------- + // + // Temporary variables used for testing: + // + // Right now we have three global variables defined at the beginning of this file + // for testing purposes: det_serving_cell, synchronize_yes and neigh_for_fbsb_det. + // + // det_serving_cell allows for detection of FBSB on serving cell, something which we + // thought should be simpler as the clock is already synched on that cell. + // + // synchronize_yes is a variable with which one can control whether offset and synch + // commands will be sent to the TPU. There was a thought that perhaps the DSP SB + // detection task for dedicated mode might not require synching due to which this + // test variable was created. Also, naturally, detection on the serving cell should + // not require synching but we could be wrong. + // + // neigh_for_fbsb_det is a variable to hardcode the neighbour one wants to detect + // FBSB on. Note that because more functionality is added to the code the mechanism + // for traversing through neighbours using select_neigh_cell and certain flags is + // disturbed for now. Due to this the index is hardcoded in the code below to + // whichever neighbour one wants to detect FBSB on (ordered by signal strength) + // using this variable. + // + // Progress so far: + // + // Right now we are able to detect FB in dedicated mode even for neighbours. One + // additional thing we have done to aid us is that we have added 3 more frames to + // our idle frame by taking up some TCH channels (this can be seen in mframe_sched). + // The call doesn't get dropped if synchronization is not performed except for very + // few cases which we need not worry about for now. However, when synchronization + // is performed the call gets dropped without exception. + // + // Few points where the problem could lie: + // + // 1) Perhaps we are not using the TCH_SB_DSP_TASK correctly. Perhaps some other + // locations in the API also need to be written to when issuing the command in + // addition to dsp_api.db_w->d_task_md. Perhaps we are not reading from the correct + // location when checking the response. + // + // 2) Perhaps we need to start the SB detection task some quarter-bits or timeslots + // before the actual SB appears. + // + // 3) Timing issues. This is the most likely cause as we haven't really been able + // to fully understand and implement timing (if it is required, which it seems like + // it is) in the code. If timing is required then this code is quite superficial. + // In idle mode functions such as afc_correct and procedures requiring frequency + // difference are used to before calling SB detection task which are not done here. + // + // Timing issues seems to be the most likely problem. Anyone wishing to solve the + // SB detection issue should try to focus on this problem, understand how + // synchronization is performed in idle mode and how we can do that in dedicated + // mode and be back within 4 frames as after that the traffic needs to resume. + // + //---------------------------------------------------------------------------------------- + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + index = l1s.neigh_sb.index; + } + +// //MTZ - putting this for now as we wanted to repeatedly detect the remaining ones - remove +// while (!((l1s.neigh_sb.flags_bsic[index] & NEIGH_PM_FLAG_BSIC) == 0)) { +//// printf("\nMTZ: BSIC has been decoded for ARFCN %d (flags_bsic[%d] = %d)\n\n", l1s.neigh_pm.band_arfcn[index], index, l1s.neigh_sb.flags_bsic[index]); +// l1s.neigh_sb.count = 0; +// l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; +// select_neigh_cell(); +// index = l1s.neigh_sb.index; +// } + + //MTZ - This index variable is used to hardcode the neighbour cell for now + //index = neigh_for_fbsb_det; + + if (sb_det == 0) { + + //l1s.fb.mode = fb_mode; + +// if (det_serving_cell) +// printf(" - detect FB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); +// else +// printf(" - detect FB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_FB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 5); //MTZ - Original - don't think works + printf("\nMTZ: arfcn in l1s_neigh_fbsb_cmd (FB) = %d\n", l1s.neigh_pm.band_arfcn[index]); + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_FB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + } else if ((sb_det == 2)||(sb_det == 4)) { + +// if (det_serving_cell) +// printf(" - detect SB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); +// else +// printf(" - detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + //MTZ - This is a variable for the testing phase whether to send sync commands or not + //if (synchronize_yes) + // synchronize_tdma2(); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + +// //MTZ - Experimenting +// dsp_api.ndb->a_sync_demod[D_TOA] = nb_fb_toa; +// dsp_api.ndb->a_sync_demod[D_PM] = nb_fb_pm; +// dsp_api.ndb->a_sync_demod[D_ANGLE] = nb_fb_angle; +// dsp_api.ndb->a_sync_demod[D_SNR] = nb_fb_snr; + + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); //MTZ - Original - don't think works + printf("\nMTZ: arfcn in l1s_neigh_fbsb_cmd (SB) = %d\n", l1s.neigh_pm.band_arfcn[index]); + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_SB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + } + return 0; +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + uint8_t bsic; + int sb_found = 0; + + if (sb_det == 0) { + if (!dsp_api.ndb->d_fb_det) { +// printf("MTZ: ARFCN %d (index %d, power %d dbm, try #%d) FB found = 0\n", l1s.neigh_pm.band_arfcn[index], index, rxlev2dbm(l1s.neigh_pm.level[index]), l1s.neigh_sb.count); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + + } else { + nb_fb_toa = dsp_api.ndb->a_sync_demod[D_TOA]; + nb_fb_pm = dsp_api.ndb->a_sync_demod[D_PM]; + nb_fb_angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + nb_fb_snr = dsp_api.ndb->a_sync_demod[D_SNR]; + printf("\n\nMTZ: ARFCN %d (index %d, power %d dbm, try #%d) FB found = 1 >>> nb_fb_toa = %d\n\n", l1s.neigh_pm.band_arfcn[index], index, rxlev2dbm(l1s.neigh_pm.level[index]), l1s.neigh_sb.count, nb_fb_toa); + sb_det = 1; + } + + //l1s_reset_hw(); + tdma_sched_reset(); + } else { + + if ((sb_det == 2)||(sb_det == 4)) { + /* check if sync was successful */ + //if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + // printf("\nSB found1 = 0\n"); + //} else { + // printf("\nSB found1 = 1\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + //} + + if (dsp_api.ndb->a_sch26[0] & (1<<B_SCH_CRC)) { +// printf("\nSB found = 0 (ARFCN %d, power %d dbm)\n\n", l1s.neigh_pm.band_arfcn[index], rxlev2dbm(l1s.neigh_pm.level[index])); + } else { + sb_found = 1; + sb = dsp_api.ndb->a_sch26[3] | dsp_api.ndb->a_sch26[4] << 16; + bsic = (sb >> 2) & 0x3f; + total_sb_det++; + //printf("=> SB 0x%08"PRIx32": BSIC=%u \n\n", sb, bsic); + printf("\n----------------------------------------------------------------------------\nSB found = 1 (ARFCN %d, power %d dbm) => SB 0x%08"PRIx32": BSIC=%u, TOA=%d (Total=%d)\n----------------------------------------------------------------------------\n\n", l1s.neigh_pm.band_arfcn[index], rxlev2dbm(l1s.neigh_pm.level[index]), sb, bsic, dsp_api.db_r->a_serv_demod[D_TOA], total_sb_det); + l1s.neigh_sb.flags_bsic[index] = bsic | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + if ((l1s.new_dm == 1) && (l1s.neigh_pm.band_arfcn[index] != 58)) { + l1s.ho_arfcn = l1s.neigh_pm.band_arfcn[index]; + l1s.new_dm = 0; + } + } + + if ((sb_det == 2)||(sb_det == 4)) { + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50); + l1s.tpu_offset = old_tpu_offset; + afc_correct(-1*ANGLE_TO_FREQ(nb_fb_angle), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + + if (synchronize_yes) { + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + + if ((sb_det == 4)||(sb_found == 1)) { + l1s.neigh_sb.count = 0; + //MTZ - need to change this statement based on detection + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + if (sb_found == 0) + printf("\n\n"); + } + + } + + if ((sb_det == 4)||(sb_found == 1)) + sb_det = 0; + else + sb_det++; + } + + return 0; +} + +static int l1s_neigh_sb_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\n\n\nMTZ: In neigh_sb_cmd, l1s.neigh_pm.n = %d\n\n\n", l1s.neigh_pm.n); + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + return 0; +} + +static int l1s_neigh_sb_resp(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + + if (l1s.neigh_pm.n == 0 || !l1s.neigh_sb.running) + goto out; + + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("SB error arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + } else { + l1s.neigh_sb.count = 0; + + read_sb_result(last_fb, 1); + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + l1s.neigh_sb.flags_bsic[index] = + l1s_decode_sb(&fbs.mon.time, sb) + | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + printf("SB OK!!!!!! arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* store time offset */ + } + +out: + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + printf("\nMTZ: In l1s_neigh_sb_resp, l1s.serving_cell.arfcn = %d\n", l1s.serving_cell.arfcn); + + return 0; + +} + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(l1s_neigh_sb_cmd, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_neigh_sb_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +const struct tdma_sched_item neigh_sync_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_fbsb_sync, 1, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_neigh_fbsb_cmd, 1, 0, 1), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_fbsb_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(sync_test1, 1, 0, 1), SCHED_END_FRAME(), +//// SCHED_END_FRAME(), +//// SCHED_END_FRAME(), +//// SCHED_ITEM_DT(sync_test2, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + + +static __attribute__ ((constructor)) void l1s_prim_fbsb_init(void) +{ + l1s.completion[L1_COMPL_FB] = &l1a_fb_compl; +} diff --git a/src/target/firmware/layer1/prim_pm.c b/src/target/firmware/layer1/prim_pm.c index 5c8c4914..6f079048 100644 --- a/src/target/firmware/layer1/prim_pm.c +++ b/src/target/firmware/layer1/prim_pm.c @@ -156,11 +156,43 @@ void l1s_pm_test(uint8_t base_fn, uint16_t arfcn) } /* - * perform measurements of neighbour cells + * perform measurements of neighbour cells on idle frame */ +/* send measurement results */ +static void neigh_pm_ind(void) +{ + struct msgb *msg; + struct l1ctl_neigh_pm_ind *mi; + int i; + uint8_t half_rounds = l1s.neigh_pm.rounds >> 1; + + /* return result */ + msg = l1ctl_msgb_alloc(L1CTL_NEIGH_PM_IND); + for (i = 0; i < l1s.neigh_pm.n; i++) { + if (msgb_tailroom(msg) < (int) sizeof(*mi)) { + l1_queue_for_l2(msg); + msg = l1ctl_msgb_alloc(L1CTL_NEIGH_PM_IND); + } + mi = (struct l1ctl_neigh_pm_ind *) + msgb_put(msg, sizeof(*mi)); + mi->band_arfcn = htons(l1s.neigh_pm.band_arfcn[i]); + mi->tn = l1s.neigh_pm.tn[i]; + l1s.neigh_pm.level[i] + = (l1s.neigh_pm.level_sum[i] + half_rounds) + / l1s.neigh_pm.rounds; + mi->pm[0] = l1s.neigh_pm.level[i]; + l1s.neigh_pm.level_sum[i] = 0; + if ((l1s.neigh_sb.flags_bsic[i] & NEIGH_PM_FLAG_BSIC)) + mi->bsic = l1s.neigh_sb.flags_bsic[i] & 0x3f; + else + mi->bsic = 255; + } + l1_queue_for_l2(msg); +} + /* scheduler callback to issue a power measurement task to the DSP */ -static int l1s_neigh_pm_cmd(uint8_t num_meas, +static int l1s_neigh_pm_idle_cmd(uint8_t num_meas, __unused uint8_t p2, __unused uint16_t p3) { uint8_t last_gain = rffe_get_gain(); @@ -188,7 +220,7 @@ static int l1s_neigh_pm_cmd(uint8_t num_meas, } /* scheduler callback to read power measurement resposnse from the DSP */ -static int l1s_neigh_pm_resp(__unused uint8_t p1, __unused uint8_t p2, +static int l1s_neigh_pm_idle_resp(__unused uint8_t p1, __unused uint8_t p2, __unused uint16_t p3) { uint16_t dbm; @@ -202,29 +234,140 @@ static int l1s_neigh_pm_resp(__unused uint8_t p1, __unused uint8_t p2, dbm = (uint16_t) ((dsp_api.db_r->a_pm[0] & 0xffff) >> 3); level = dbm2rxlev(agc_inp_dbm8_by_pm(dbm)/8); - l1s.neigh_pm.level[l1s.neigh_pm.pos] = level; + l1s.neigh_pm.level_sum[l1s.neigh_pm.pos] = level; if (++l1s.neigh_pm.pos >= l1s.neigh_pm.n) { - struct msgb *msg; - struct l1ctl_neigh_pm_ind *mi; - int i; - l1s.neigh_pm.pos = 0; - /* return result */ - msg = l1ctl_msgb_alloc(L1CTL_NEIGH_PM_IND); - for (i = 0; i < l1s.neigh_pm.n; i++) { - if (msgb_tailroom(msg) < (int) sizeof(*mi)) { - l1_queue_for_l2(msg); - msg = l1ctl_msgb_alloc(L1CTL_NEIGH_PM_IND); + l1s.neigh_pm.rounds++; + neigh_pm_ind(); + l1s.neigh_pm.rounds = 0; + l1s.neigh_pm.valid = 1; + } + +out: + l1s.neigh_pm.running = 0; + + return 0; +} + +const struct tdma_sched_item neigh_pm_idle_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_pm_idle_cmd, -1, 1, 0), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_pm_idle_resp, -4, 1, 0), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +/* + * Perform measurements of neighbour cells on TCH + * + * Only when number of neighbor cells is > 0, perform measurement. + * + * For each measurement, l1s.neigh_pm.running is set. In case of an update + * of neighbor cell list, this state is cleared, so a pending measurement would + * be ignored. + * + * The measuement starts at position 0 (first neighbor cell). After each + * measurement result, the position is incremented until the number of neighbor + * cells are reached. + * + * All measurement results are added to level_sum array. It will be used to + * calculate an average from multiple measurements. + * + * All measurements start at round 0. When all neighbors have been measured, + * the number of rounds is increased and the measurement start over. At start + * of round 0, the start_fn is recorded. It will be used to calculate the + * elapsed time from the beginning. + * + * After reach round, the number of elapsed frames is checked. If at least 104 + * frames have been elapsed, this would be the last round. The average of + * measurements are calculated from the level_sum array. Then the result is + * indicated to layer, the measurement states are reset, the measurments are + * maked as valid, and the measurement process starts over. + */ + +/* scheduler callback to issue a power measurement task to the DSP */ +static int l1s_neigh_pm_tch_cmd(uint8_t num_meas, + __unused uint8_t p2, __unused uint16_t p3) +{ + uint8_t last_gain; + + if (l1s.neigh_pm.n == 0) + return 0; + + /* set start_fn for the first round */ + if (l1s.neigh_pm.pos == 0 && l1s.neigh_pm.rounds == 0) + l1s.neigh_pm.start_fn = l1s.next_time.fn; + + /* save current gain */ + last_gain = rffe_get_gain(); + + dsp_api.db_w->d_task_md = num_meas; /* number of measurements */ +// dsp_api.ndb->d_fb_mode = 0; /* wideband search */ + + /* Tell the RF frontend to set the gain appropriately (keep last) */ + rffe_compute_gain(-85, CAL_DSP_TGT_BB_LVL); + + + /* + * Program TPU + * Use TS 5 (two TS after TX) + */ + l1s_rx_win_ctrl((l1s.neigh_pm.n) ? + l1s.neigh_pm.band_arfcn[l1s.neigh_pm.pos] : 0, + L1_RXWIN_PW, 5); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_pm.running = 1; + + return 0; +} + +/* scheduler callback to read power measurement resposnse from the DSP */ +static int l1s_neigh_pm_tch_resp(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + uint16_t dbm; + uint8_t level; + + dsp_api.r_page_used = 1; + + if (l1s.neigh_pm.n == 0 || !l1s.neigh_pm.running) + goto out; + + dbm = (uint16_t) ((dsp_api.db_r->a_pm[0] & 0xffff) >> 3); + level = dbm2rxlev(agc_inp_dbm8_by_pm(dbm)/8); + + //MTZ: The code below is to artificially induce handover + ////////////////////////////////////////////// + if (((l1s.serving_cell.arfcn == 25) && ((l1s.neigh_pm.band_arfcn[l1s.neigh_pm.pos] == 57)||(l1s.neigh_pm.band_arfcn[l1s.neigh_pm.pos] == 52)||(l1s.neigh_pm.band_arfcn[l1s.neigh_pm.pos] == 49))) || ((l1s.serving_cell.arfcn == 57) && ((l1s.neigh_pm.band_arfcn[l1s.neigh_pm.pos] == 25)||(l1s.neigh_pm.band_arfcn[l1s.neigh_pm.pos] == 52)||(l1s.neigh_pm.band_arfcn[l1s.neigh_pm.pos] == 58)))) { + l1s.neigh_pm.level_sum[l1s.neigh_pm.pos] += 63; + } else { + l1s.neigh_pm.level_sum[l1s.neigh_pm.pos] += level; + } + /////////////////////////////////////////////// + + + //MTZ: This if statement is used because it was seen that sometimes a level of 0 was read off in dedicated mode which is not valid + if (level > 0) { + if (++l1s.neigh_pm.pos >= l1s.neigh_pm.n) { + uint32_t elapsed = (l1s.next_time.fn + 2715648 + - l1s.neigh_pm.start_fn) % 2715648; + + l1s.neigh_pm.pos = 0; + l1s.neigh_pm.rounds++; + /* + * We want at least 104 frames before indicating the + * measurement(s). Add two, since the measurement was + * started two frames ago. + */ + if (elapsed >= 104 + 2) { + neigh_pm_ind(); + l1s.neigh_pm.rounds = 0; + l1s.neigh_pm.valid = 1; } - mi = (struct l1ctl_neigh_pm_ind *) - msgb_put(msg, sizeof(*mi)); - mi->band_arfcn = htons(l1s.neigh_pm.band_arfcn[i]); - mi->tn = l1s.neigh_pm.tn[i]; - mi->pm[0] = l1s.neigh_pm.level[i]; - mi->pm[1] = 0; } - l1_queue_for_l2(msg); } out: @@ -233,10 +376,11 @@ out: return 0; } -const struct tdma_sched_item neigh_pm_sched_set[] = { - SCHED_ITEM_DT(l1s_neigh_pm_cmd, 0, 1, 0), SCHED_END_FRAME(), +/* NOTE: Prio 1 is below TCH's TX+RX prio 0 */ +const struct tdma_sched_item neigh_pm_tch_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_pm_tch_cmd, 1, 1, 0), SCHED_END_FRAME(), SCHED_END_FRAME(), - SCHED_ITEM(l1s_neigh_pm_resp, -4, 1, 0), SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_pm_tch_resp, -4, 1, 0), SCHED_END_FRAME(), SCHED_END_SET() }; diff --git a/src/target/firmware/layer1/prim_rach.c b/src/target/firmware/layer1/prim_rach.c index e6ea6568..97736ec9 100644 --- a/src/target/firmware/layer1/prim_rach.c +++ b/src/target/firmware/layer1/prim_rach.c @@ -50,6 +50,10 @@ #include <l1ctl_proto.h> +int hando_access_flag = 0; +int16_t hando_arfcn = 0; +int ho_flag = 0; + struct { uint32_t fn; uint16_t band_arfcn; @@ -60,11 +64,18 @@ static int l1s_tx_rach_cmd(__unused uint8_t p1, __unused uint8_t p2, __unused ui { uint16_t *info_ptr; uint16_t arfcn; + uint8_t tsc, tn; uint8_t data[2]; putchart('T'); - l1s_tx_apc_helper(l1s.serving_cell.arfcn); + if (hando_access_flag == 1) + arfcn = hando_arfcn; + else + arfcn = l1s.serving_cell.arfcn; + + //l1s_tx_apc_helper(l1s.serving_cell.arfcn); + l1s_tx_apc_helper(arfcn); data[0] = l1s.serving_cell.bsic << 2; data[1] = l1s.rach.ra; @@ -72,7 +83,11 @@ static int l1s_tx_rach_cmd(__unused uint8_t p1, __unused uint8_t p2, __unused ui info_ptr = &dsp_api.ndb->d_rach; info_ptr[0] = ((uint16_t)(data[0])) | ((uint16_t)(data[1])<<8); - arfcn = l1s.serving_cell.arfcn; + //rfch_get_params(&l1s.next_time, &arfcn, &tsc, &tn); //MTZ - ADDED LATER - POSSIBLY REMOVE + + //hando_access_flag = 0; + + printf("\n\nMTZ - arfcn in l1s_tx_rach_cmd = %d, serving cell arfcn = %d\n\n", arfcn, l1s.serving_cell.arfcn); dsp_api.db_w->d_task_ra = dsp_task_iq_swap(RACH_DSP_TASK, arfcn, 1); @@ -85,14 +100,50 @@ static int l1s_tx_rach_cmd(__unused uint8_t p1, __unused uint8_t p2, __unused ui static int l1s_tx_rach_resp(__unused uint8_t p1, __unused uint8_t burst_id, __unused uint16_t p3) { + uint16_t arfcn; + + if (hando_access_flag == 1) + arfcn = hando_arfcn; + else + arfcn = l1s.serving_cell.arfcn; + + hando_access_flag = 0; + putchart('t'); dsp_api.r_page_used = 1; + if (ho_flag == 1) { + ho_flag = 0; + int ii = 0; + for (ii =0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == arfcn) { + if (l1s.nb_sb_snr[ii] > 0) + afc_input(l1s.nb_sb_freq_diff[ii], arfcn, 1); + else + afc_input(l1s.nb_sb_freq_diff[ii], arfcn, 0); + + + //mframe_disable(MF_TASK_TCH_F_EVEN); + //mframe_disable(MF_TASK_TCH_F_ODD); + //mframe_disable(MF_TASK_TCH_H_0); + //mframe_disable(MF_TASK_TCH_H_1); + + //mframe_disable(MF_TASK_NEIGH_PM51_C0T0); + //mframe_disable(MF_TASK_NEIGH_PM51); + //mframe_disable(MF_TASK_NEIGH_PM26E); + //mframe_disable(MF_TASK_NEIGH_PM26O); + + //mframe_enable(MF_TASK_BCCH_NORM); + + } + } + } + /* schedule a confirmation back indicating the GSM time at which * the RACH burst was transmitted to the BTS */ last_rach.fn = l1s.current_time.fn - 1; - last_rach.band_arfcn = l1s.serving_cell.arfcn; + last_rach.band_arfcn = arfcn; l1s_compl_sched(L1_COMPL_RACH); return 0; @@ -111,6 +162,7 @@ static void l1a_rach_compl(__unused enum l1_compl c) { struct msgb *msg; + printf("\n\nMTZ: L1CTL_RACH_CONF message being sent!\n\n\n"); msg = l1_create_l2_msg(L1CTL_RACH_CONF, last_rach.fn, 0, last_rach.band_arfcn); l1_queue_for_l2(msg); @@ -131,16 +183,72 @@ static uint8_t rach_to_t3_comb[27] = { 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 45, 46}; -/* request a RACH request at the next multiframe T3 = fn51 */ -void l1a_rach_req(uint16_t offset, uint8_t combined, uint8_t ra) +/* schedule access burst */ +void l1a_rach_req(uint16_t offset, uint8_t combined, uint8_t ra, uint16_t arfcn) { uint32_t fn_sched; unsigned long flags; + if (arfcn > 0) { + hando_arfcn = arfcn; + hando_access_flag = 1; + //printf("\n\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\nMTZ: CP1\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n"); + uint32_t new_fn; + if (l1s.new_dm) { + int ii = 0; + for (ii =0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == arfcn) { + printf("\n\n\n\n\n\n\n-------------------------------------------\n\n\n\n\nMTZ: SYNCHING TO NEW ARFCN %d\n\n\n\n\n-------------------------------------------\n\n\n\n\n\n\n\n", arfcn); + //printf("\n\n\n\n\n\n\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!MTZ: SYNCHING TO NEW ARFCN %d (offset=%d, freq_diff=%d, sb_fb_freq_diff=%d, sb_snr=%d, frame_diff=%d)!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n\n\n\n\n\n\n", arfcn, l1s.tpu_offsets[ii], l1s.nb_freq_diff[ii], l1s.nb_sb_freq_diff[ii], l1s.nb_sb_snr[ii], l1s.nb_frame_diff[ii]); + + //MTZ - Setting tpu_offset for frame start synchronization as well as doing frequency compensation + l1s.orig_tpu_offset = l1s.tpu_offset; + l1s.tpu_offset = l1s.tpu_offsets[ii]; + afc_correct(l1s.nb_freq_diff[ii], arfcn); + + //if (l1s.nb_sb_snr[ii] > 0) + // afc_input(l1s.nb_sb_freq_diff[ii], arfcn, 1); + //else + // afc_input(l1s.nb_sb_freq_diff[ii], arfcn, 0); + + //MTZ - Setting frame number using the difference computed before + new_fn = l1s.current_time.fn + l1s.nb_frame_diff[ii]; + while (new_fn < 0) + new_fn += 2715647; + new_fn = new_fn % 2715647; + gsm_fn2gsmtime(&l1s.current_time, new_fn); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + tdma_sched_reset(); + + ho_flag = 1; + + } + } + l1s.new_dm = 0; + } + } + offset += 3; + //printf("\n\nMTZ: In l1a_rach_req, serving cell = %d\n\n", l1s.serving_cell.arfcn); + local_firq_save(flags); - if (combined) { + if (l1s.dedicated.type == GSM_DCHAN_TCH_F) { + fn_sched = l1s.current_time.fn + offset; + /* go next DCCH frame TCH/F channel */ + if ((fn_sched % 13) == 12) + fn_sched++; + } else if (l1s.dedicated.type == GSM_DCHAN_TCH_H) { + fn_sched = l1s.current_time.fn + offset; + /* go next DCCH frame of TCH/H channel */ + if ((fn_sched % 13) == 12) + fn_sched++; + if ((l1s.dedicated.chan_nr & 1) != ((fn_sched % 13) & 1)) + fn_sched++; + } else if (combined) { /* add elapsed RACH slots to offset */ offset += t3_to_rach_comb[l1s.current_time.t3]; /* offset is the number of RACH slots in the future */ @@ -150,6 +258,7 @@ void l1a_rach_req(uint16_t offset, uint8_t combined, uint8_t ra) } else fn_sched = l1s.current_time.fn + offset; l1s.rach.ra = ra; + fn_sched %= 2715648; sched_gsmtime(rach_sched_set_ul, fn_sched, 0); local_irq_restore(flags); diff --git a/src/target/firmware/layer1/prim_rach_tmp.c b/src/target/firmware/layer1/prim_rach_tmp.c new file mode 100644 index 00000000..e2ab9488 --- /dev/null +++ b/src/target/firmware/layer1/prim_rach_tmp.c @@ -0,0 +1,265 @@ +/* Layer 1 Random Access Channel Burst */ + +/* (C) 2010 by Dieter Spaar <spaar@mirider.augusta.de> + * (C) 2010 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> + +#include <defines.h> +#include <debug.h> +#include <memory.h> +#include <byteorder.h> +#include <osmocom/gsm/gsm_utils.h> +#include <osmocom/core/msgb.h> +#include <calypso/dsp_api.h> +#include <calypso/irq.h> +#include <calypso/tpu.h> +#include <calypso/tsp.h> +#include <calypso/dsp.h> +#include <calypso/timer.h> +#include <comm/sercomm.h> +#include <asm/system.h> + +#include <layer1/sync.h> +#include <layer1/async.h> +#include <layer1/tdma_sched.h> +#include <layer1/tpu_window.h> +#include <layer1/l23_api.h> +#include <layer1/sched_gsmtime.h> + +#include <l1ctl_proto.h> + +int hando_access_flag = 0; +int16_t hando_arfcn = 0; +int ho_flag = 0; + +struct { + uint32_t fn; + uint16_t band_arfcn; +} last_rach; + +/* p1: type of operation (0: one NB, 1: one RACH burst, 2: four NB */ +static int l1s_tx_rach_cmd(__unused uint8_t p1, __unused uint8_t p2, __unused uint16_t p3) +{ + uint16_t *info_ptr; + uint16_t arfcn; + uint8_t tsc, tn; + uint8_t data[2]; + + putchart('T'); + + if (hando_access_flag == 1) + arfcn = hando_arfcn; + else + arfcn = l1s.serving_cell.arfcn; + + //l1s_tx_apc_helper(l1s.serving_cell.arfcn); + l1s_tx_apc_helper(arfcn); + + data[0] = l1s.serving_cell.bsic << 2; + data[1] = l1s.rach.ra; + + info_ptr = &dsp_api.ndb->d_rach; + info_ptr[0] = ((uint16_t)(data[0])) | ((uint16_t)(data[1])<<8); + + //rfch_get_params(&l1s.next_time, &arfcn, &tsc, &tn); //MTZ - ADDED LATER - POSSIBLY REMOVE + + //hando_access_flag = 0; + + printf("\n\nMTZ - arfcn in l1s_tx_rach_cmd = %d, serving cell arfcn = %d\n\n", arfcn, l1s.serving_cell.arfcn); + + dsp_api.db_w->d_task_ra = dsp_task_iq_swap(RACH_DSP_TASK, arfcn, 1); + + l1s_tx_win_ctrl(arfcn | ARFCN_UPLINK, L1_TXWIN_AB, 0, 3); + + return 0; +} + +/* p1: type of operation (0: one NB, 1: one RACH burst, 2: four NB */ +static int l1s_tx_rach_resp(__unused uint8_t p1, __unused uint8_t burst_id, + __unused uint16_t p3) +{ + uint16_t arfcn; + + if (hando_access_flag == 1) + arfcn = hando_arfcn; + else + arfcn = l1s.serving_cell.arfcn; + + hando_access_flag = 0; + + putchart('t'); + + dsp_api.r_page_used = 1; + + if (ho_flag == 1) { + ho_flag = 0; + int ii = 0; + for (ii =0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == arfcn) { + if (l1s.nb_sb_snr[ii] > 0) + afc_input(l1s.nb_sb_freq_diff[ii], arfcn, 1); + else + afc_input(l1s.nb_sb_freq_diff[ii], arfcn, 0); + } + } + } + + /* schedule a confirmation back indicating the GSM time at which + * the RACH burst was transmitted to the BTS */ + last_rach.fn = l1s.current_time.fn - 1; + last_rach.band_arfcn = arfcn; + l1s_compl_sched(L1_COMPL_RACH); + + return 0; +} + +/* sched sets for uplink */ +const struct tdma_sched_item rach_sched_set_ul[] = { + SCHED_ITEM_DT(l1s_tx_rach_cmd, 3, 1, 0), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_tx_rach_resp, -4, 1, 0), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +/* Asynchronous completion handler for FB detection */ +static void l1a_rach_compl(__unused enum l1_compl c) +{ + struct msgb *msg; + + printf("\n\nMTZ: L1CTL_RACH_CONF message being sent!\n\n\n"); + msg = l1_create_l2_msg(L1CTL_RACH_CONF, last_rach.fn, 0, + last_rach.band_arfcn); + l1_queue_for_l2(msg); +} + +static uint8_t t3_to_rach_comb[51] = { + 0, 0, 0, 0, + 0, 1, + 2, 2, 2, 2, 2, 2, 2, 2, + 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, + 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, + 25, 25, 25, 25, 25, 25, 25, 25, + 25, 26, + 27, 27, 27, 27}; +static uint8_t rach_to_t3_comb[27] = { + 4, 5, + 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, + 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, + 45, 46}; + +/* schedule access burst */ +void l1a_rach_req(uint16_t offset, uint8_t combined, uint8_t ra, uint16_t arfcn) +{ + uint32_t fn_sched; + unsigned long flags; + + if (arfcn > 0) { + hando_arfcn = arfcn; + hando_access_flag = 1; + printf("\n\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\nMTZ: CP1\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n"); + uint32_t new_fn; + if (l1s.new_dm2) { + int ii = 0; + for (ii =0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == arfcn) { + printf("\n\n\n\n----------------------------------------------------------\n\n\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!MTZ: SYNCHING TO NEW ARFCN %d (offset=%d, freq_diff=%d, sb_fb_freq_diff=%d, sb_snr=%d, frame_diff=%d)!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n----------------------------------------------------------\n\n\n\n\n\n", arfcn, l1s.tpu_offsets[ii], l1s.nb_freq_diff[ii], l1s.nb_sb_freq_diff[ii], l1s.nb_sb_snr[ii], l1s.nb_frame_diff[ii]); + l1s.orig_tpu_offset = l1s.tpu_offset; + l1s.tpu_offset = l1s.tpu_offsets[ii]; + afc_correct(l1s.nb_freq_diff[ii], arfcn); + + new_fn = l1s.current_time.fn + l1s.nb_frame_diff[ii]; + if (new_fn < 0) + new_fn += 2715647; + new_fn = new_fn % 2715647; + gsm_fn2gsmtime(&l1s.current_time, new_fn); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + tdma_sched_reset(); + + ho_flag = 1; + + //if (l1s.nb_sb_snr[ii] > 0) + // afc_input(l1s.nb_sb_freq_diff[ii], arfcn, 1); + //else + // afc_input(l1s.nb_sb_freq_diff[ii], arfcn, 0); + + //mframe_disable(MF_TASK_TCH_F_EVEN); + //mframe_disable(MF_TASK_TCH_F_ODD); + //mframe_disable(MF_TASK_TCH_H_0); + //mframe_disable(MF_TASK_TCH_H_1); + + //mframe_disable(MF_TASK_NEIGH_PM51_C0T0); + //mframe_disable(MF_TASK_NEIGH_PM51); + //mframe_disable(MF_TASK_NEIGH_PM26E); + //mframe_disable(MF_TASK_NEIGH_PM26O); + + //mframe_enable(MF_TASK_BCCH_NORM); + + } + } + l1s.new_dm2 = 0; + } + } + + offset += 3; + + //printf("\n\nMTZ: In l1a_rach_req, serving cell = %d\n\n", l1s.serving_cell.arfcn); + + local_firq_save(flags); + if (l1s.dedicated.type == GSM_DCHAN_TCH_F) { + fn_sched = l1s.current_time.fn + offset; + /* go next DCCH frame TCH/F channel */ + if ((fn_sched % 13) == 12) + fn_sched++; + } else if (l1s.dedicated.type == GSM_DCHAN_TCH_H) { + fn_sched = l1s.current_time.fn + offset; + /* go next DCCH frame of TCH/H channel */ + if ((fn_sched % 13) == 12) + fn_sched++; + if ((l1s.dedicated.chan_nr & 1) != ((fn_sched % 13) & 1)) + fn_sched++; + } else if (combined) { + /* add elapsed RACH slots to offset */ + offset += t3_to_rach_comb[l1s.current_time.t3]; + /* offset is the number of RACH slots in the future */ + fn_sched = l1s.current_time.fn - l1s.current_time.t3; + fn_sched += offset / 27 * 51; + fn_sched += rach_to_t3_comb[offset % 27]; + } else + fn_sched = l1s.current_time.fn + offset; + l1s.rach.ra = ra; + fn_sched %= 2715648; + sched_gsmtime(rach_sched_set_ul, fn_sched, 0); + local_irq_restore(flags); + + memset(&last_rach, 0, sizeof(last_rach)); +} + +static __attribute__ ((constructor)) void prim_rach_init(void) +{ + l1s.completion[L1_COMPL_RACH] = &l1a_rach_compl; +} diff --git a/src/target/firmware/layer1/prim_rx_nb.c b/src/target/firmware/layer1/prim_rx_nb.c index 38c7b53b..cb77d3bc 100644 --- a/src/target/firmware/layer1/prim_rx_nb.c +++ b/src/target/firmware/layer1/prim_rx_nb.c @@ -89,6 +89,9 @@ static int l1s_nb_resp(__unused uint8_t p1, uint8_t burst_id, uint16_t p3) gsm_fn2gsmtime(&rx_time, l1s.current_time.fn - 1); rfch_get_params(&rx_time, &rf_arfcn, &tsc, &tn); + //As causing held issues + //printf("\nMTZ:l1s_nb_resp, sc=%d\n", l1s.serving_cell.arfcn); + /* collect measurements */ rxnb.meas[burst_id].toa_qbit = dsp_api.db_r->a_serv_demod[D_TOA]; rxnb.meas[burst_id].pm_dbm8 = @@ -204,6 +207,7 @@ static int l1s_nb_cmd(__unused uint8_t p1, uint8_t burst_id, burst_id, tsc ); + //printf("\nMTZ: arfcn in l1s_nb_cmd = %d\n", arfcn); l1s_rx_win_ctrl(arfcn, L1_RXWIN_NB, 0); return 0; diff --git a/src/target/firmware/layer1/prim_tch.c b/src/target/firmware/layer1/prim_tch.c index a8036d2f..808c2d3b 100644 --- a/src/target/firmware/layer1/prim_tch.c +++ b/src/target/firmware/layer1/prim_tch.c @@ -56,6 +56,9 @@ #include <l1ctl_proto.h> +/* FIXME: count of what? use better name */ +static int count11 = 0; + static inline int msb_get_bit(uint8_t *buf, int bn) { int pos_byte = bn >> 3; @@ -213,6 +216,13 @@ static int l1s_tch_resp(__unused uint8_t p1, __unused uint8_t p2, uint16_t p3) meas_id = (meas_id + 1) % FACCH_MEAS_HIST; /* absolute value doesn't matter */ + count11++; + if (count11 == 20){ + count11=0; + //printf("\nMTZ: arfcn in l1s_tch_resp = %d, sc = %d\n", arfcn, l1s.serving_cell.arfcn); + } + //printf("\nMTZ: arfcn in l1s_tch_resp = %d, sc = %d\n", arfcn, l1s.serving_cell.arfcn); + /* Collect measurements */ rx_tch.meas[meas_id].toa_qbit = dsp_api.db_r->a_serv_demod[D_TOA]; rx_tch.meas[meas_id].pm_dbm8 = @@ -258,6 +268,9 @@ static int l1s_tch_resp(__unused uint8_t p1, __unused uint8_t p2, uint16_t p3) /* Allocate msgb */ /* FIXME: we actually want all allocation out of L1S! */ msg = l1ctl_msgb_alloc(L1CTL_DATA_IND); + //MTZ + //printf("\n\n\n---------------------------------\nFACCH\n---------------------------------\n\n\n"); + //printf("\n\n\n---------------------------------\nFACCH\n---------------------------------\n\n\n"); if(!msg) { printf("TCH FACCH: unable to allocate msgb\n"); goto skip_rx_facch; @@ -272,6 +285,8 @@ static int l1s_tch_resp(__unused uint8_t p1, __unused uint8_t p2, uint16_t p3) dl->band_arfcn = htons(arfcn); dl->frame_nr = htonl(rx_time.fn); + printf("\n\nMTZ: FACCH received - chan_nr=%x\n\n", chan_nr); + /* Average SNR & RX level */ n = tch_f_hn ? 8 : 6; for (i=0; i<n; i++) { @@ -429,8 +444,10 @@ static int l1s_tch_cmd(__unused uint8_t p1, __unused uint8_t p2, uint16_t p3) msg = NULL; /* If TX is empty and we're signalling only, use dummy frame */ - if (msg) - data = msg->l3h; + if (msg) { //MTZ + //printf("\n\n\n---------------------------------\nFACCH\n---------------------------------\n\n\n"); + //printf("\n\n\n---------------------------------\nFACCH\n---------------------------------\n\n\n"); + data = msg->l3h;} else if (tch_mode == SIG_ONLY_MODE) data = pu_get_idle_frame(); else @@ -533,8 +550,16 @@ skip_tx_traffic: dsp_task_iq_swap(TCHT_DSP_TASK, arfcn, 0), 0, tsc /* burst_id unused for TCH */ ); + + //printf("\nMTZ: arfcn in l1s_tch_cmd = %d, sc = %d\n", arfcn, l1s.serving_cell.arfcn); + l1s_rx_win_ctrl(arfcn, L1_RXWIN_NB, 0); + /* stop here, if TX is disabled */ + if (l1s.dedicated.rx_only) + return 0; + + dsp_load_tx_task( dsp_task_iq_swap(TCHT_DSP_TASK, arfcn, 1), 0, tsc /* burst_id unused for TCH */ @@ -798,8 +823,14 @@ static int l1s_tch_a_cmd(__unused uint8_t p1, __unused uint8_t p2, uint16_t p3) dsp_task_iq_swap(TCHA_DSP_TASK, arfcn, 0), 0, tsc /* burst_id unused for TCHA */ ); + //printf("\nMTZ: arfcn in l1s_tch_a_cmd = %d, sc=%d\n", arfcn, l1s.serving_cell.arfcn); l1s_rx_win_ctrl(arfcn, L1_RXWIN_NB, 0); + /* stop here, if TX is disabled */ + if (l1s.dedicated.rx_only) + return 0; + + dsp_load_tx_task( dsp_task_iq_swap(TCHA_DSP_TASK, arfcn, 1), 0, tsc /* burst_id unused for TCHA */ diff --git a/src/target/firmware/layer1/prim_tx_nb.c b/src/target/firmware/layer1/prim_tx_nb.c index 86e8224c..2c27be6c 100644 --- a/src/target/firmware/layer1/prim_tx_nb.c +++ b/src/target/firmware/layer1/prim_tx_nb.c @@ -125,6 +125,7 @@ static int l1s_tx_cmd(uint8_t p1, uint8_t burst_id, uint16_t p3) burst_id, tsc ); + //printf("\nMTZ: arfcn in l1s_tx_cmd = %d, sc=%d\n", arfcn, l1s.serving_cell.arfcn); l1s_tx_win_ctrl(arfcn | ARFCN_UPLINK, L1_TXWIN_NB, 0, 3); return 0; diff --git a/src/target/firmware/layer1/sync.c b/src/target/firmware/layer1/sync.c index 36f42975..6c39f51e 100644 --- a/src/target/firmware/layer1/sync.c +++ b/src/target/firmware/layer1/sync.c @@ -187,6 +187,8 @@ void l1s_reset_hw(void) static int last_timestamp; +static int last_ts; + static inline void check_lost_frame(void) { int diff, timestamp = hwtimer_read(1); @@ -195,6 +197,26 @@ static inline void check_lost_frame(void) last_timestamp += (4*TIMER_TICKS_PER_TDMA); diff = last_timestamp - timestamp; + + /* TS change compensation */ + if (l1s.dedicated.type) { + if (l1s.dedicated.tn < last_ts) { + int ediff = ((8 - last_ts + l1s.dedicated.tn) * TIMER_TICKS_PER_TDMA) >> 3; + printf("TS Chg back: %d -> %d | %d %d\n", + last_ts, l1s.dedicated.tn, diff, ediff); + + // if (((ediff - 2) < diff) && ((ediff + 2) > diff)) { + puts("ADV !\n"); + l1s.current_time = l1s.next_time; + l1s_time_inc(&l1s.next_time, 1); + // } + } else if (l1s.dedicated.tn > last_ts) + printf("TS Chg forth: %d -> %d | %d\n", + last_ts, l1s.dedicated.tn, diff); + last_ts = l1s.dedicated.tn; + } +// } else +// last_ts = 0; /* allow for a bit of jitter */ if (diff < TIMER_TICKS_PER_TDMA - TIMER_TICK_JITTER || @@ -367,6 +389,7 @@ void l1s_reset(void) /* Leave dedicated mode */ l1s.dedicated.type = GSM_DCHAN_NONE; + last_ts = 0; /* reset scheduler and hardware */ sched_gsmtime_reset(); @@ -382,10 +405,20 @@ void l1s_init(void) { unsigned int i; + for (i = 0; i < 64; i++) { + l1s.tpu_offsets_arfcn[i] = 0; + l1s.tpu_offsets[i] = 0; + l1s.nb_freq_diff[i] = 0; + l1s.nb_sb_freq_diff[i] = 0; + l1s.nb_sb_snr[i] = 0; + l1s.nb_frame_diff[i] = 0; + } + for (i = 0; i < ARRAY_SIZE(l1s.tx_queue); i++) INIT_LLIST_HEAD(&l1s.tx_queue[i]); l1s.tx_meas = NULL; + sched_gsmtime_init(); /* register FRAME interrupt as FIQ so it can interrupt normal IRQs */ diff --git a/src/target/firmware/layer1/tmpfbsb.c b/src/target/firmware/layer1/tmpfbsb.c new file mode 100644 index 00000000..f7656139 --- /dev/null +++ b/src/target/firmware/layer1/tmpfbsb.c @@ -0,0 +1,1017 @@ +/* Layer 1 - FCCH and SCH burst handling */ + +/* (C) 2010 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <inttypes.h> +#include <defines.h> +#include <debug.h> +#include <memory.h> +#include <byteorder.h> +#include <rffe.h> +#include <osmocom/gsm/gsm_utils.h> +#include <osmocom/core/msgb.h> +#include <calypso/dsp_api.h> +#include <calypso/irq.h> +#include <calypso/tpu.h> +#include <calypso/tsp.h> +#include <calypso/dsp.h> +#include <calypso/timer.h> +#include <comm/sercomm.h> + +#include <layer1/sync.h> +#include <layer1/afc.h> +#include <layer1/toa.h> +#include <layer1/tdma_sched.h> +#include <layer1/mframe_sched.h> +#include <layer1/tpu_window.h> +#include <layer1/l23_api.h> +#include <layer1/agc.h> + +#include <l1ctl_proto.h> + +#define FB0_RETRY_COUNT 3 +#define AFC_RETRY_COUNT 30 + +extern uint16_t rf_arfcn; // TODO + +struct mon_state { + uint32_t fnr_report; /* frame number when DSP reported it */ + int attempt; /* which attempt was this ? */ + + int16_t toa; + uint16_t pm; + uint16_t angle; + uint16_t snr; + + /* computed values */ + int16_t freq_diff; + + /* Sync Burst (SB) */ + uint8_t bsic; + struct gsm_time time; +}; + +struct l1a_fb_state { + struct mon_state mon; + struct l1ctl_fbsb_req req; + int16_t initial_freq_err; + uint8_t fb_retries; + uint8_t afc_retries; +}; + +static struct l1a_fb_state fbs; +static struct mon_state *last_fb = &fbs.mon; +static int sb_det = 0; //MTZ - This was added +uint32_t old_tpu_offset = 0; //MTZ - This was added + +int16_t nb_fb_toa = 0; //MTZ - This was added +uint16_t nb_fb_pm = 0; //MTZ - This was added +uint16_t nb_fb_angle = 0; //MTZ - This was added +uint16_t nb_fb_snr = 0; //MTZ - This was added + +// MTZ - for working of these variables see comments in l1s_neigh_fbsb_cmd +// MTZ - det_serving_cell overrides neigh_for_fbsb_det +int synchronize_yes = 0; //MTZ - A test variable +int det_serving_cell = 0; //MTZ - A test variable +int neigh_for_fbsb_det = 0; //MTZ - A test variable + +static void dump_mon_state(struct mon_state *fb) +{ +#if 0 + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz, " + "SNR=%04x(%d.%u) OFFSET=%u SYNCHRO=%u\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle), + fb->snr, l1s_snr_int(fb->snr), l1s_snr_fract(fb->snr), + tpu_get_offset(), tpu_get_synchro()); +#else + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle)); +#endif +} + +static int l1ctl_fbsb_resp(uint8_t res) +{ + struct msgb *msg; + struct l1ctl_fbsb_conf *resp; + + msg = l1_create_l2_msg(L1CTL_FBSB_CONF, fbs.mon.time.fn, + l1s_snr_int(fbs.mon.snr), + fbs.req.band_arfcn); + if (!msg) + return -ENOMEM; + + resp = (struct l1ctl_fbsb_conf *) msgb_put(msg, sizeof(*resp)); + resp->initial_freq_err = htons(fbs.initial_freq_err); + resp->result = res; + resp->bsic = fbs.mon.bsic; + + /* no need to set BSIC, as it is never used here */ + l1_queue_for_l2(msg); + + return 0; +} + +/* SCH Burst Detection ********************************************************/ + +/* determine the GSM time and BSIC from a Sync Burst */ +static uint8_t l1s_decode_sb(struct gsm_time *time, uint32_t sb) +{ + uint8_t bsic = (sb >> 2) & 0x3f; + uint8_t t3p; + + memset(time, 0, sizeof(*time)); + + /* TS 05.02 Chapter 3.3.2.2.1 SCH Frame Numbers */ + time->t1 = ((sb >> 23) & 1) | ((sb >> 7) & 0x1fe) | ((sb << 9) & 0x600); + time->t2 = (sb >> 18) & 0x1f; + t3p = ((sb >> 24) & 1) | ((sb >> 15) & 6); + time->t3 = t3p*10 + 1; + + /* TS 05.02 Chapter 4.3.3 TDMA frame number */ + time->fn = gsm_gsmtime2fn(time); + + time->tc = (time->fn / 51) % 8; + + return bsic; +} + +static void read_sb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.db_r->a_serv_demod[D_TOA]; + st->pm = dsp_api.db_r->a_serv_demod[D_PM]>>3; + st->angle = dsp_api.db_r->a_serv_demod[D_ANGLE]; + st->snr = dsp_api.db_r->a_serv_demod[D_SNR]; + + st->freq_diff = ANGLE_TO_FREQ(st->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + if (st->snr > AFC_SNR_THRESHOLD) + afc_input(st->freq_diff, rf_arfcn, 1); + else + afc_input(st->freq_diff, rf_arfcn, 0); + + dsp_api.r_page_used = 1; +} + +/* Note: When we get the SB response, it is 2 TDMA frames after the SB + * actually happened, as it is a "C W W R" task */ +#define SB2_LATENCY 2 + +static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, + __unused uint16_t p3) +{ + uint32_t sb; + int qbits, fn_offset; + struct l1_cell_info *cinfo = &l1s.serving_cell; + int fnr_delta, bits_delta; + + putchart('s'); + + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + /* mark READ page as being used */ + dsp_api.r_page_used = 1; + + /* after 2nd attempt, we failed */ + if (attempt == 2) { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + /* after 1st attempt, we simply wait for 2nd */ + return 0; + } + + printf("SB%d ", attempt); + read_sb_result(last_fb, attempt); + + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + fbs.mon.bsic = l1s_decode_sb(&fbs.mon.time, sb); + printf("=> SB 0x%08"PRIx32": BSIC=%u ", sb, fbs.mon.bsic); + l1s_time_dump(&fbs.mon.time); + + l1s.serving_cell.bsic = fbs.mon.bsic; + + /* calculate synchronisation value (TODO: only complete for qbits) */ + last_fb->toa -= 23; + qbits = last_fb->toa * 4; + fn_offset = l1s.current_time.fn; // TODO + + if (qbits > QBITS_PER_TDMA) { + qbits -= QBITS_PER_TDMA; + fn_offset -= 1; + } else if (qbits < 0) { + qbits += QBITS_PER_TDMA; + fn_offset += 1; + } + + fnr_delta = last_fb->fnr_report - attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports SB in bit that is %d bits in the " + "future?!?\n", last_fb->toa - bits_delta); + else + printf(" qbits=%u\n", qbits); + + synchronize_tdma(&l1s.serving_cell); + + /* if we have recived a SYNC burst, update our local GSM time */ + gsm_fn2gsmtime(&l1s.current_time, fbs.mon.time.fn + SB2_LATENCY); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + /* If we call tdma_sched_reset(), which is only needed if there + * are further l1s_sbdet_resp() scheduled, we will bring + * dsp_api.db_r and dsp_api.db_w out of sync because we changed + * dsp_api.db_w for l1s_sbdet_cmd() and canceled + * l1s_sbdet_resp() which would change dsp_api.db_r. The DSP + * however expects dsp_api.db_w and dsp_api.db_r to be in sync + * (either "0 - 0" or "1 - 1"). So we have to bring dsp_api.db_w + * and dsp_api.db_r into sync again, otherwise NB reading will + * complain. We probably don't need the Abort command and could + * just bring dsp_api.db_w and dsp_api.db_r into sync. */ + if (attempt != 2) { + tdma_sched_reset(); + l1s_dsp_abort(); + } + + l1s_reset_hw(); + /* enable the MF Task for BCCH reading */ + mframe_enable(MF_TASK_BCCH_NORM); + if (l1s.serving_cell.ccch_mode == CCCH_MODE_COMBINED) + mframe_enable(MF_TASK_CCCH_COMB); + else if (l1s.serving_cell.ccch_mode == CCCH_MODE_NON_COMBINED) + mframe_enable(MF_TASK_CCCH); + + l1s_compl_sched(L1_COMPL_FB); + + return 0; +} + +static int l1s_sbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + putchart('S'); + + fbs.mon.bsic = 0; + fbs.mon.time.fn = 0; + + dsp_api.db_w->d_task_md = SB_DSP_TASK; + dsp_api.ndb->d_fb_mode = 0; /* wideband search */ + + /* Program TPU */ + l1s_rx_win_ctrl(rf_arfcn, L1_RXWIN_SB, 0); + + return 0; +} + +/* This is how it is done by the TSM30 */ +static const struct tdma_sched_item sb_sched_set[] = { + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 2), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +void l1s_sb_test(uint8_t base_fn) +{ + tdma_schedule_set(base_fn, sb_sched_set, 0); +} +/* FCCH Burst *****************************************************************/ + +static int read_fb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.ndb->a_sync_demod[D_TOA]; + st->pm = dsp_api.ndb->a_sync_demod[D_PM]>>3; + st->angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + st->snr = dsp_api.ndb->a_sync_demod[D_SNR]; + + //last_fb->angle = clip_int16(last_fb->angle, AFC_MAX_ANGLE); + st->freq_diff = ANGLE_TO_FREQ(last_fb->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + dsp_api.ndb->d_fb_det = 0; + dsp_api.ndb->a_sync_demod[D_TOA] = 0; /* TSM30 does it (really needed ?) */ + + /* Update AFC with current frequency offset */ + afc_correct(st->freq_diff, rf_arfcn); + + //tpu_dsp_frameirq_enable(); + return 1; +} + +static void fbinfo2cellinfo(struct l1_cell_info *cinfo, + const struct mon_state *mon) +{ + int ntdma, qbits, fn_offset, fnr_delta, bits_delta; + + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + fnr_delta = last_fb->fnr_report - last_fb->attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports FB in bit that is %d bits in " + "the future?!?\n", last_fb->toa - bits_delta); + else { + int fb_fnr = (last_fb->fnr_report - last_fb->attempt) + + last_fb->toa/BITS_PER_TDMA; + printf("=>FB @ FNR %u fn_offset=%d qbits=%u\n", + fb_fnr, fn_offset, qbits); + } +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_fbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + if (fb_mode == 0) { + putchart('F'); + } else { + putchart('V'); + } + + l1s.fb.mode = fb_mode; + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = FB_DSP_TASK; /* maybe with I/Q swap? */ + dsp_api.ndb->d_fb_mode = fb_mode; + + /* Program TPU */ + l1s_rx_win_ctrl(fbs.req.band_arfcn, L1_RXWIN_FB, 0); + + return 0; +} + +#if 0 +#define FB0_SNR_THRESH 2000 +#define FB1_SNR_THRESH 3000 +#else +#define FB0_SNR_THRESH 0 +#define FB1_SNR_THRESH 0 +#endif + +static const struct tdma_sched_item fb_sched_set[]; + +/* scheduler callback to check for a FB detection response */ +static int l1s_fbdet_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + putchart('f'); + + if (!dsp_api.ndb->d_fb_det) { + /* we did not detect a FB */ + + /* attempt < 12, do nothing */ + if (attempt < 12) + return 0; + + /* attempt >= 12, we simply don't find one */ + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + if (fbs.fb_retries < FB0_RETRY_COUNT) { + /* retry once more */ + tdma_schedule_set(1, fb_sched_set, 0); + fbs.fb_retries++; + } else { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; + } + + /* We found a frequency burst, reset everything */ + l1s_reset_hw(); + + printf("FB%u ", dsp_api.ndb->d_fb_mode); + read_fb_result(last_fb, attempt); + + /* if this is the first success, save freq err */ + if (!fbs.initial_freq_err) + fbs.initial_freq_err = last_fb->freq_diff; + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + /* Immediately schedule further TDMA tasklets, if requested. Doing + * this directly from L1S means we can do this quickly without any + * additional delays */ + if (fb_mode == 0) { + if (fbs.req.flags & L1CTL_FBSB_F_FB1) { + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + /* FIXME: don't only use the last but an average */ + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh1 && + last_fb->snr > FB0_SNR_THRESH) { + /* continue with FB1 task in DSP */ + tdma_schedule_set(1, fb_sched_set, 1); + } else { + if (fbs.afc_retries < AFC_RETRY_COUNT) { + tdma_schedule_set(1, fb_sched_set, 0); + fbs.afc_retries++; + } else { + /* Abort */ + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + } + } else + l1s_compl_sched(L1_COMPL_FB); + } else if (fb_mode == 1) { + if (fbs.req.flags & L1CTL_FBSB_F_SB) { + + int ntdma, qbits; + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + + int fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + int delay = fn_offset + 11 - l1s.current_time.fn - 1; + printf(" fn_offset=%d (fn=%"PRIu32" + attempt=%u + ntdma = %d)\n", + fn_offset, l1s.current_time.fn, last_fb->attempt, ntdma); + printf(" delay=%d (fn_offset=%d + 11 - fn=%"PRIu32" - 1\n", delay, + fn_offset, l1s.current_time.fn); + printf(" scheduling next FB/SB detection task with delay %u\n", delay); + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh2 && + last_fb->snr > FB1_SNR_THRESH) { + /* synchronize before reading SB */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + synchronize_tdma(&l1s.serving_cell); + tdma_schedule_set(delay, sb_sched_set, 0); + } else + tdma_schedule_set(delay, fb_sched_set, 1); + } else + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; +} + +/* FB detection */ +static const struct tdma_sched_item fb_sched_set[] = { + SCHED_ITEM_DT(l1s_fbdet_cmd, 0, 0, 0), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 3), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 4), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 5), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 6), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 7), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 8), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 9), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 10), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 11), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 12), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +/* Asynchronous completion handler for FB detection */ +static void l1a_fb_compl(__unused enum l1_compl c) +{ + if (last_fb->attempt >= 13) { + /* FB detection failed, signal this via L1CTL */ + l1ctl_fbsb_resp(255); + return; + } + + /* FIME: use l1s.neigh_cell[fbs.cinfo_idx] */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + + /* send FBSB_CONF success message via L1CTL */ + l1ctl_fbsb_resp(0); +} + +void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req) +{ + /* copy + endian convert request data */ + fbs.req.band_arfcn = ntohs(req->band_arfcn); + fbs.req.timeout = ntohs(req->timeout); + fbs.req.freq_err_thresh1 = ntohs(req->freq_err_thresh1); + fbs.req.freq_err_thresh2 = ntohs(req->freq_err_thresh2); + fbs.req.num_freqerr_avg = req->num_freqerr_avg; + fbs.req.flags = req->flags; + fbs.req.sync_info_idx = req->sync_info_idx; + fbs.req.rxlev_exp = req->rxlev_exp; + + /* clear initial frequency error */ + fbs.initial_freq_err = 0; + fbs.fb_retries = 0; + fbs.afc_retries = 0; + + /* Make sure we start at a 'center' AFCDAC output value */ + afc_reset(); + + /* Reset the TOA loop counters */ + toa_reset(); + + if (fbs.req.flags & L1CTL_FBSB_F_FB0) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_FB1) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_SB) + tdma_schedule_set(base_fn, sb_sched_set, 0); + +} + +/* SB for Neighbours in dedicated mode + * + * Only when number of neighbor cells is > 0, perform synchronization. + * + * For each synchronization, l1s.neigh_pm.running is set. In case of an update + * of neighbor cell list, this state is cleared, so a pending sync result would + * be ignored. + * + * After a (new) list of neighbor cells are received, the measurements are not + * yet valid. A valid state flag is used to indicate valid measurements. Until + * there are no valid measurements, the synchronization is not performed. + * + * The task is to scan the 6 strongest neighbor cells by trying to synchronize + * to it. This is done by selecting the strongest unscanned neighbor cell. + * If 6 cells have been scanned or all cells (if less than 6) have been + * scanned, the process clears all 'scanned' flags and starts over with the + * strongest (now the strongest unscanned) cell. + * + * Each synchronization attempt is performed during the "search frame" (IDLE + * frame). The process attempts to sync 11 times to ensure that it hits the + * SCH of the neighbor's BCCH. (The alignment of BCCH shifts after every two + * 26-multiframe in a way that the "search frame" is aligned with the SCH, at + * least once for 11 successive "search frames".) + * + * If the synchronization attempt is successful, the BSIC and neighbor cell + * offset is stored. These are indicated to layer23 with the measurement + * results. + * + * When performing handover to a neighbor cell, the stored offset is used to + * calculate new GSM time and tpu_offset. + */ + +static void select_neigh_cell(void) +{ + uint8_t strongest = 0, strongest_unscanned = 0; + int strongest_i = 0, strongest_unscanned_i = -1; + int num_scanned = 0; + int i; + + /* find strongest cell and strongest unscanned cell and count */ + for (i = 0; i < l1s.neigh_pm.n; i++) { + if (l1s.neigh_pm.level[i] > strongest) { + strongest = l1s.neigh_pm.level[i]; + strongest_i = i; + } + if (!(l1s.neigh_sb.flags_bsic[i] & NEIGH_PM_FLAG_SCANNED)) { + if (l1s.neigh_pm.level[i] > strongest_unscanned) { + strongest_unscanned = l1s.neigh_pm.level[i]; + strongest_unscanned_i = i; + } + } else + num_scanned++; + } + + /* no unscanned cell or we have scanned enough */ + if (strongest_unscanned_i < 0 || num_scanned >= 6) { + /* flag all cells unscanned */ + for (i = 0; i < l1s.neigh_pm.n; i++) + l1s.neigh_sb.flags_bsic[i] &= ~NEIGH_PM_FLAG_SCANNED; + /* use strongest cell to begin scanning with */ + l1s.neigh_sb.index = strongest_i; + } else { + /* use strongest unscanned cell to begin scanning with */ + l1s.neigh_sb.index = strongest_unscanned_i; + } +} + +//MTZ - The function below has been taken from synchronize_tdma in sync.c and modified for whatever seemed necessary +void synchronize_tdma2() +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + + //if (tpu_instr_test) { //MTZ - uncomment + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + //} +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\nMTZ: In neigh_fbsb_cmd, l1s.neigh_pm.n = %d\n", l1s.neigh_pm.n); + + //---------------------------------------------------------------------------------------- + // + // Important points of note: + // ------------------------- + // + // Temporary variables used for testing: + // + // Right now we have three global variables defined at the beginning of this file + // for testing purposes: det_serving_cell, synchronize_yes and neigh_for_fbsb_det. + // + // det_serving_cell allows for detection of FBSB on serving cell, something which we + // thought should be simpler as the clock is already synched on that cell. + // + // synchronize_yes is a variable with which one can control whether offset and synch + // commands will be sent to the TPU. There was a thought that perhaps the DSP SB + // detection task for dedicated mode might not require synching due to which this + // test variable was created. Also, naturally, detection on the serving cell should + // not require synching but we could be wrong. + // + // neigh_for_fbsb_det is a variable to hardcode the neighbour one wants to detect + // FBSB on. Note that because more functionality is added to the code the mechanism + // for traversing through neighbours using select_neigh_cell and certain flags is + // disturbed for now. Due to this the index is hardcoded in the code below to + // whichever neighbour one wants to detect FBSB on (ordered by signal strength) + // using this variable. + // + // Progress so far: + // + // Right now we are able to detect FB in dedicated mode even for neighbours. One + // additional thing we have done to aid us is that we have added 3 more frames to + // our idle frame by taking up some TCH channels (this can be seen in mframe_sched). + // The call doesn't get dropped if synchronization is not performed except for very + // few cases which we need not worry about for now. However, when synchronization + // is performed the call gets dropped without exception. + // + // Few points where the problem could lie: + // + // 1) Perhaps we are not using the TCH_SB_DSP_TASK correctly. Perhaps some other + // locations in the API also need to be written to when issuing the command in + // addition to dsp_api.db_w->d_task_md. Perhaps we are not reading from the correct + // location when checking the response. + // + // 2) Perhaps we need to start the SB detection task some quarter-bits or timeslots + // before the actual SB appears. + // + // 3) Timing issues. This is the most likely cause as we haven't really been able + // to fully understand and implement timing (if it is required, which it seems like + // it is) in the code. If timing is required then this code is quite superficial. + // In idle mode functions such as afc_correct and procedures requiring frequency + // difference are used to before calling SB detection task which are not done here. + // + // Timing issues seems to be the most likely problem. Anyone wishing to solve the + // SB detection issue should try to focus on this problem, understand how + // synchronization is performed in idle mode and how we can do that in dedicated + // mode and be back within 4 frames as after that the traffic needs to resume. + // + //---------------------------------------------------------------------------------------- + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + //MTZ - This index variable is used to hardcode the neighbour cell for now + index = neigh_for_fbsb_det; + + if (sb_det == 0) { + + //l1s.fb.mode = fb_mode; + + if (det_serving_cell) + printf("detect FB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); + else + printf("detect FB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_FB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 5); //MTZ - Original - don't think works + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_FB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + } else if (sb_det == 2) { + + if (det_serving_cell) + printf("detect SB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); + else + printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + //MTZ - This is a variable for the testing phase whether to send sync commands or not + if (synchronize_yes) + synchronize_tdma2(); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + +// //MTZ - Experimenting +// dsp_api.ndb->a_sync_demod[D_TOA] = nb_fb_toa; +// dsp_api.ndb->a_sync_demod[D_PM] = nb_fb_pm; +// dsp_api.ndb->a_sync_demod[D_ANGLE] = nb_fb_angle; +// dsp_api.ndb->a_sync_demod[D_SNR] = nb_fb_snr; + + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); //MTZ - Original - don't think works + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_SB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + } + return 0; +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + if (sb_det == 0) { + if (!dsp_api.ndb->d_fb_det) { + printf("\nMTZ: FB found = 0\n\n"); + } else { + nb_fb_toa = dsp_api.ndb->a_sync_demod[D_TOA]; + nb_fb_pm = dsp_api.ndb->a_sync_demod[D_PM]; + nb_fb_angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + nb_fb_snr = dsp_api.ndb->a_sync_demod[D_SNR]; + printf("\nMTZ: FB found = 1, FB%u, nb_fb_toa = %d\n\n", dsp_api.ndb->d_fb_mode, nb_fb_toa); + sb_det = 1; + } + + //l1s_reset_hw(); + tdma_sched_reset(); + } else { + + if (sb_det == 2) { + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("\nSB found1 = 0\n\n"); + } else { + printf("\nSB found1 = 1\n\n"); + } + + if (dsp_api.ndb->a_sch26[0] & (1<<B_SCH_CRC)) { + printf("\nSB found2 = 0\n\n"); + } else { + printf("\nSB found2 = 1\n\n"); + } + + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + if (synchronize_yes) { + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + + sb_det++; + if (sb_det == 3) + sb_det = 0; + } + + return 0; +} + +static int l1s_neigh_sb_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\n\n\nMTZ: In neigh_sb_cmd, l1s.neigh_pm.n = %d\n\n\n", l1s.neigh_pm.n); + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + return 0; +} + +static int l1s_neigh_sb_resp(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + + if (l1s.neigh_pm.n == 0 || !l1s.neigh_sb.running) + goto out; + + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("SB error arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + } else { + l1s.neigh_sb.count = 0; + + read_sb_result(last_fb, 1); + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + l1s.neigh_sb.flags_bsic[index] = + l1s_decode_sb(&fbs.mon.time, sb) + | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + printf("SB OK!!!!!! arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* store time offset */ + } + +out: + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + return 0; + +} + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(l1s_neigh_sb_cmd, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_neigh_sb_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +const struct tdma_sched_item neigh_sync_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_fbsb_cmd, 1, 0, 1), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_fbsb_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + + +static __attribute__ ((constructor)) void l1s_prim_fbsb_init(void) +{ + l1s.completion[L1_COMPL_FB] = &l1a_fb_compl; +} diff --git a/src/target/firmware/layer1/tmpfbsbWorking.c b/src/target/firmware/layer1/tmpfbsbWorking.c new file mode 100644 index 00000000..0baf2e9d --- /dev/null +++ b/src/target/firmware/layer1/tmpfbsbWorking.c @@ -0,0 +1,937 @@ +/* Layer 1 - FCCH and SCH burst handling */ + +/* (C) 2010 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <inttypes.h> +#include <defines.h> +#include <debug.h> +#include <memory.h> +#include <byteorder.h> +#include <rffe.h> +#include <osmocom/gsm/gsm_utils.h> +#include <osmocom/core/msgb.h> +#include <calypso/dsp_api.h> +#include <calypso/irq.h> +#include <calypso/tpu.h> +#include <calypso/tsp.h> +#include <calypso/dsp.h> +#include <calypso/timer.h> +#include <comm/sercomm.h> + +#include <layer1/sync.h> +#include <layer1/afc.h> +#include <layer1/toa.h> +#include <layer1/tdma_sched.h> +#include <layer1/mframe_sched.h> +#include <layer1/tpu_window.h> +#include <layer1/l23_api.h> +#include <layer1/agc.h> + +#include <l1ctl_proto.h> + +#define FB0_RETRY_COUNT 3 +#define AFC_RETRY_COUNT 30 + +extern uint16_t rf_arfcn; // TODO + +struct mon_state { + uint32_t fnr_report; /* frame number when DSP reported it */ + int attempt; /* which attempt was this ? */ + + int16_t toa; + uint16_t pm; + uint16_t angle; + uint16_t snr; + + /* computed values */ + int16_t freq_diff; + + /* Sync Burst (SB) */ + uint8_t bsic; + struct gsm_time time; +}; + +struct l1a_fb_state { + struct mon_state mon; + struct l1ctl_fbsb_req req; + int16_t initial_freq_err; + uint8_t fb_retries; + uint8_t afc_retries; +}; + +static struct l1a_fb_state fbs; +static struct mon_state *last_fb = &fbs.mon; +static int sb_det = 0; //MTZ - This was added +uint32_t old_tpu_offset = 0; //MTZ - This was added + +int16_t nb_fb_toa = 0; //MTZ - This was added +uint16_t nb_fb_pm = 0; //MTZ - This was added +uint16_t nb_fb_angle = 0; //MTZ - This was added +uint16_t nb_fb_snr = 0; //MTZ - This was added + +static void dump_mon_state(struct mon_state *fb) +{ +#if 0 + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz, " + "SNR=%04x(%d.%u) OFFSET=%u SYNCHRO=%u\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle), + fb->snr, l1s_snr_int(fb->snr), l1s_snr_fract(fb->snr), + tpu_get_offset(), tpu_get_synchro()); +#else + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle)); +#endif +} + +static int l1ctl_fbsb_resp(uint8_t res) +{ + struct msgb *msg; + struct l1ctl_fbsb_conf *resp; + + msg = l1_create_l2_msg(L1CTL_FBSB_CONF, fbs.mon.time.fn, + l1s_snr_int(fbs.mon.snr), + fbs.req.band_arfcn); + if (!msg) + return -ENOMEM; + + resp = (struct l1ctl_fbsb_conf *) msgb_put(msg, sizeof(*resp)); + resp->initial_freq_err = htons(fbs.initial_freq_err); + resp->result = res; + resp->bsic = fbs.mon.bsic; + + /* no need to set BSIC, as it is never used here */ + l1_queue_for_l2(msg); + + return 0; +} + +/* SCH Burst Detection ********************************************************/ + +/* determine the GSM time and BSIC from a Sync Burst */ +static uint8_t l1s_decode_sb(struct gsm_time *time, uint32_t sb) +{ + uint8_t bsic = (sb >> 2) & 0x3f; + uint8_t t3p; + + memset(time, 0, sizeof(*time)); + + /* TS 05.02 Chapter 3.3.2.2.1 SCH Frame Numbers */ + time->t1 = ((sb >> 23) & 1) | ((sb >> 7) & 0x1fe) | ((sb << 9) & 0x600); + time->t2 = (sb >> 18) & 0x1f; + t3p = ((sb >> 24) & 1) | ((sb >> 15) & 6); + time->t3 = t3p*10 + 1; + + /* TS 05.02 Chapter 4.3.3 TDMA frame number */ + time->fn = gsm_gsmtime2fn(time); + + time->tc = (time->fn / 51) % 8; + + return bsic; +} + +static void read_sb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.db_r->a_serv_demod[D_TOA]; + st->pm = dsp_api.db_r->a_serv_demod[D_PM]>>3; + st->angle = dsp_api.db_r->a_serv_demod[D_ANGLE]; + st->snr = dsp_api.db_r->a_serv_demod[D_SNR]; + + st->freq_diff = ANGLE_TO_FREQ(st->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + if (st->snr > AFC_SNR_THRESHOLD) + afc_input(st->freq_diff, rf_arfcn, 1); + else + afc_input(st->freq_diff, rf_arfcn, 0); + + dsp_api.r_page_used = 1; +} + +/* Note: When we get the SB response, it is 2 TDMA frames after the SB + * actually happened, as it is a "C W W R" task */ +#define SB2_LATENCY 2 + +static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, + __unused uint16_t p3) +{ + uint32_t sb; + int qbits, fn_offset; + struct l1_cell_info *cinfo = &l1s.serving_cell; + int fnr_delta, bits_delta; + + putchart('s'); + + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + /* mark READ page as being used */ + dsp_api.r_page_used = 1; + + /* after 2nd attempt, we failed */ + if (attempt == 2) { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + /* after 1st attempt, we simply wait for 2nd */ + return 0; + } + + printf("SB%d ", attempt); + read_sb_result(last_fb, attempt); + + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + fbs.mon.bsic = l1s_decode_sb(&fbs.mon.time, sb); + printf("=> SB 0x%08"PRIx32": BSIC=%u ", sb, fbs.mon.bsic); + l1s_time_dump(&fbs.mon.time); + + l1s.serving_cell.bsic = fbs.mon.bsic; + + /* calculate synchronisation value (TODO: only complete for qbits) */ + last_fb->toa -= 23; + qbits = last_fb->toa * 4; + fn_offset = l1s.current_time.fn; // TODO + + if (qbits > QBITS_PER_TDMA) { + qbits -= QBITS_PER_TDMA; + fn_offset -= 1; + } else if (qbits < 0) { + qbits += QBITS_PER_TDMA; + fn_offset += 1; + } + + fnr_delta = last_fb->fnr_report - attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports SB in bit that is %d bits in the " + "future?!?\n", last_fb->toa - bits_delta); + else + printf(" qbits=%u\n", qbits); + + synchronize_tdma(&l1s.serving_cell); + + /* if we have recived a SYNC burst, update our local GSM time */ + gsm_fn2gsmtime(&l1s.current_time, fbs.mon.time.fn + SB2_LATENCY); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + /* If we call tdma_sched_reset(), which is only needed if there + * are further l1s_sbdet_resp() scheduled, we will bring + * dsp_api.db_r and dsp_api.db_w out of sync because we changed + * dsp_api.db_w for l1s_sbdet_cmd() and canceled + * l1s_sbdet_resp() which would change dsp_api.db_r. The DSP + * however expects dsp_api.db_w and dsp_api.db_r to be in sync + * (either "0 - 0" or "1 - 1"). So we have to bring dsp_api.db_w + * and dsp_api.db_r into sync again, otherwise NB reading will + * complain. We probably don't need the Abort command and could + * just bring dsp_api.db_w and dsp_api.db_r into sync. */ + if (attempt != 2) { + tdma_sched_reset(); + l1s_dsp_abort(); + } + + l1s_reset_hw(); + /* enable the MF Task for BCCH reading */ + mframe_enable(MF_TASK_BCCH_NORM); + if (l1s.serving_cell.ccch_mode == CCCH_MODE_COMBINED) + mframe_enable(MF_TASK_CCCH_COMB); + else if (l1s.serving_cell.ccch_mode == CCCH_MODE_NON_COMBINED) + mframe_enable(MF_TASK_CCCH); + + l1s_compl_sched(L1_COMPL_FB); + + return 0; +} + +static int l1s_sbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + putchart('S'); + + fbs.mon.bsic = 0; + fbs.mon.time.fn = 0; + + dsp_api.db_w->d_task_md = SB_DSP_TASK; + dsp_api.ndb->d_fb_mode = 0; /* wideband search */ + + /* Program TPU */ + l1s_rx_win_ctrl(rf_arfcn, L1_RXWIN_SB, 0); + + return 0; +} + +/* This is how it is done by the TSM30 */ +static const struct tdma_sched_item sb_sched_set[] = { + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 2), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +void l1s_sb_test(uint8_t base_fn) +{ + tdma_schedule_set(base_fn, sb_sched_set, 0); +} +/* FCCH Burst *****************************************************************/ + +static int read_fb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.ndb->a_sync_demod[D_TOA]; + st->pm = dsp_api.ndb->a_sync_demod[D_PM]>>3; + st->angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + st->snr = dsp_api.ndb->a_sync_demod[D_SNR]; + + //last_fb->angle = clip_int16(last_fb->angle, AFC_MAX_ANGLE); + st->freq_diff = ANGLE_TO_FREQ(last_fb->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + dsp_api.ndb->d_fb_det = 0; + dsp_api.ndb->a_sync_demod[D_TOA] = 0; /* TSM30 does it (really needed ?) */ + + /* Update AFC with current frequency offset */ + afc_correct(st->freq_diff, rf_arfcn); + + //tpu_dsp_frameirq_enable(); + return 1; +} + +static void fbinfo2cellinfo(struct l1_cell_info *cinfo, + const struct mon_state *mon) +{ + int ntdma, qbits, fn_offset, fnr_delta, bits_delta; + + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + fnr_delta = last_fb->fnr_report - last_fb->attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports FB in bit that is %d bits in " + "the future?!?\n", last_fb->toa - bits_delta); + else { + int fb_fnr = (last_fb->fnr_report - last_fb->attempt) + + last_fb->toa/BITS_PER_TDMA; + printf("=>FB @ FNR %u fn_offset=%d qbits=%u\n", + fb_fnr, fn_offset, qbits); + } +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_fbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + if (fb_mode == 0) { + putchart('F'); + } else { + putchart('V'); + } + + l1s.fb.mode = fb_mode; + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = FB_DSP_TASK; /* maybe with I/Q swap? */ + dsp_api.ndb->d_fb_mode = fb_mode; + + /* Program TPU */ + l1s_rx_win_ctrl(fbs.req.band_arfcn, L1_RXWIN_FB, 0); + + return 0; +} + +#if 0 +#define FB0_SNR_THRESH 2000 +#define FB1_SNR_THRESH 3000 +#else +#define FB0_SNR_THRESH 0 +#define FB1_SNR_THRESH 0 +#endif + +static const struct tdma_sched_item fb_sched_set[]; + +/* scheduler callback to check for a FB detection response */ +static int l1s_fbdet_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + putchart('f'); + + if (!dsp_api.ndb->d_fb_det) { + /* we did not detect a FB */ + + /* attempt < 12, do nothing */ + if (attempt < 12) + return 0; + + /* attempt >= 12, we simply don't find one */ + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + if (fbs.fb_retries < FB0_RETRY_COUNT) { + /* retry once more */ + tdma_schedule_set(1, fb_sched_set, 0); + fbs.fb_retries++; + } else { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; + } + + /* We found a frequency burst, reset everything */ + l1s_reset_hw(); + + printf("FB%u ", dsp_api.ndb->d_fb_mode); + read_fb_result(last_fb, attempt); + + /* if this is the first success, save freq err */ + if (!fbs.initial_freq_err) + fbs.initial_freq_err = last_fb->freq_diff; + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + /* Immediately schedule further TDMA tasklets, if requested. Doing + * this directly from L1S means we can do this quickly without any + * additional delays */ + if (fb_mode == 0) { + if (fbs.req.flags & L1CTL_FBSB_F_FB1) { + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + /* FIXME: don't only use the last but an average */ + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh1 && + last_fb->snr > FB0_SNR_THRESH) { + /* continue with FB1 task in DSP */ + tdma_schedule_set(1, fb_sched_set, 1); + } else { + if (fbs.afc_retries < AFC_RETRY_COUNT) { + tdma_schedule_set(1, fb_sched_set, 0); + fbs.afc_retries++; + } else { + /* Abort */ + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + } + } else + l1s_compl_sched(L1_COMPL_FB); + } else if (fb_mode == 1) { + if (fbs.req.flags & L1CTL_FBSB_F_SB) { + + int ntdma, qbits; + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + + int fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + int delay = fn_offset + 11 - l1s.current_time.fn - 1; + printf(" fn_offset=%d (fn=%"PRIu32" + attempt=%u + ntdma = %d)\n", + fn_offset, l1s.current_time.fn, last_fb->attempt, ntdma); + printf(" delay=%d (fn_offset=%d + 11 - fn=%"PRIu32" - 1\n", delay, + fn_offset, l1s.current_time.fn); + printf(" scheduling next FB/SB detection task with delay %u\n", delay); + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh2 && + last_fb->snr > FB1_SNR_THRESH) { + /* synchronize before reading SB */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + synchronize_tdma(&l1s.serving_cell); + tdma_schedule_set(delay, sb_sched_set, 0); + } else + tdma_schedule_set(delay, fb_sched_set, 1); + } else + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; +} + +/* FB detection */ +static const struct tdma_sched_item fb_sched_set[] = { + SCHED_ITEM_DT(l1s_fbdet_cmd, 0, 0, 0), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 3), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 4), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 5), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 6), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 7), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 8), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 9), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 10), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 11), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 12), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +/* Asynchronous completion handler for FB detection */ +static void l1a_fb_compl(__unused enum l1_compl c) +{ + if (last_fb->attempt >= 13) { + /* FB detection failed, signal this via L1CTL */ + l1ctl_fbsb_resp(255); + return; + } + + /* FIME: use l1s.neigh_cell[fbs.cinfo_idx] */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + + /* send FBSB_CONF success message via L1CTL */ + l1ctl_fbsb_resp(0); +} + +void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req) +{ + /* copy + endian convert request data */ + fbs.req.band_arfcn = ntohs(req->band_arfcn); + fbs.req.timeout = ntohs(req->timeout); + fbs.req.freq_err_thresh1 = ntohs(req->freq_err_thresh1); + fbs.req.freq_err_thresh2 = ntohs(req->freq_err_thresh2); + fbs.req.num_freqerr_avg = req->num_freqerr_avg; + fbs.req.flags = req->flags; + fbs.req.sync_info_idx = req->sync_info_idx; + fbs.req.rxlev_exp = req->rxlev_exp; + + /* clear initial frequency error */ + fbs.initial_freq_err = 0; + fbs.fb_retries = 0; + fbs.afc_retries = 0; + + /* Make sure we start at a 'center' AFCDAC output value */ + afc_reset(); + + /* Reset the TOA loop counters */ + toa_reset(); + + if (fbs.req.flags & L1CTL_FBSB_F_FB0) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_FB1) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_SB) + tdma_schedule_set(base_fn, sb_sched_set, 0); + +} + +/* SB for Neighbours in dedicated mode + * + * Only when number of neighbor cells is > 0, perform synchronization. + * + * For each synchronization, l1s.neigh_pm.running is set. In case of an update + * of neighbor cell list, this state is cleared, so a pending sync result would + * be ignored. + * + * After a (new) list of neighbor cells are received, the measurements are not + * yet valid. A valid state flag is used to indicate valid measurements. Until + * there are no valid measurements, the synchronization is not performed. + * + * The task is to scan the 6 strongest neighbor cells by trying to synchronize + * to it. This is done by selecting the strongest unscanned neighbor cell. + * If 6 cells have been scanned or all cells (if less than 6) have been + * scanned, the process clears all 'scanned' flags and starts over with the + * strongest (now the strongest unscanned) cell. + * + * Each synchronization attempt is performed during the "search frame" (IDLE + * frame). The process attempts to sync 11 times to ensure that it hits the + * SCH of the neighbor's BCCH. (The alignment of BCCH shifts after every two + * 26-multiframe in a way that the "search frame" is aligned with the SCH, at + * least once for 11 successive "search frames".) + * + * If the synchronization attempt is successful, the BSIC and neighbor cell + * offset is stored. These are indicated to layer23 with the measurement + * results. + * + * When performing handover to a neighbor cell, the stored offset is used to + * calculate new GSM time and tpu_offset. + */ + +static void select_neigh_cell(void) +{ + uint8_t strongest = 0, strongest_unscanned = 0; + int strongest_i = 0, strongest_unscanned_i = -1; + int num_scanned = 0; + int i; + + /* find strongest cell and strongest unscanned cell and count */ + for (i = 0; i < l1s.neigh_pm.n; i++) { + if (l1s.neigh_pm.level[i] > strongest) { + strongest = l1s.neigh_pm.level[i]; + strongest_i = i; + } + if (!(l1s.neigh_sb.flags_bsic[i] & NEIGH_PM_FLAG_SCANNED)) { + if (l1s.neigh_pm.level[i] > strongest_unscanned) { + strongest_unscanned = l1s.neigh_pm.level[i]; + strongest_unscanned_i = i; + } + } else + num_scanned++; + } + + /* no unscanned cell or we have scanned enough */ + if (strongest_unscanned_i < 0 || num_scanned >= 6) { + /* flag all cells unscanned */ + for (i = 0; i < l1s.neigh_pm.n; i++) + l1s.neigh_sb.flags_bsic[i] &= ~NEIGH_PM_FLAG_SCANNED; + /* use strongest cell to begin scanning with */ + l1s.neigh_sb.index = strongest_i; + } else { + /* use strongest unscanned cell to begin scanning with */ + l1s.neigh_sb.index = strongest_unscanned_i; + } +} + +void synchronize_tdma2() +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + + //if (tpu_instr_test) { //MTZ - uncomment + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + //} +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fb_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\n\n\nMTZ: In neigh_fb_cmd, l1s.neigh_pm.n = %d\n\n\n", l1s.neigh_pm.n); + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + index = 0; //MTZ - hardcoded - the above logic needs to be modified + + if (sb_det == 0) { + + //l1s.fb.mode = fb_mode; + + //printf("detect FB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); //MTZ - Uncomment + printf("detect FB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + //rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); //MTZ- Uncomment + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_FB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 5); + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 0); + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_FB26, 0); //MTZ - Uncomment + + /* restore last gain */ + rffe_set_gain(last_gain); + } else if (sb_det ==2) { + + //printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); //Uncomment + printf("detect SB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); + + synchronize_tdma2(); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + //rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); //MTZ - Uncomment + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + //MTZ - Experimenting + dsp_api.ndb->a_sync_demod[D_TOA] = nb_fb_toa; + dsp_api.ndb->a_sync_demod[D_PM] = nb_fb_pm; + dsp_api.ndb->a_sync_demod[D_ANGLE] = nb_fb_angle; + dsp_api.ndb->a_sync_demod[D_SNR] = nb_fb_snr; + + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 0); + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_SB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + } + return 0; +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fb_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + if (sb_det == 0) { + if (!dsp_api.ndb->d_fb_det) { + printf("\nMTZ: FB found = 0\n\n"); + } else { + nb_fb_toa = dsp_api.ndb->a_sync_demod[D_TOA]; + nb_fb_pm = dsp_api.ndb->a_sync_demod[D_PM]; + nb_fb_angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + nb_fb_snr = dsp_api.ndb->a_sync_demod[D_SNR]; + printf("\nMTZ: FB found = 1, FB%u, nb_fb_toa = %d\n\n", dsp_api.ndb->d_fb_mode, nb_fb_toa); + sb_det = 1; + } + + //l1s_reset_hw(); + tdma_sched_reset(); + } else { + + if (sb_det == 2) { + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("\nSB found1 = 0\n\n"); + } else { + printf("\nSB found1 = 1\n\n"); + } + + if (dsp_api.ndb->a_sch26[0] & (1<<B_SCH_CRC)) { + printf("\nSB found2 = 0\n\n"); + } else { + printf("\nSB found2 = 1\n\n"); + } + + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + //if (tpu_instr_test) { + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + //} + } + + sb_det++; + if (sb_det == 3) + sb_det = 0; + } + + return 0; +} + +static int l1s_neigh_sb_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\n\n\nMTZ: In neigh_sb_cmd, l1s.neigh_pm.n = %d\n\n\n", l1s.neigh_pm.n); + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + return 0; +} + +static int l1s_neigh_sb_resp(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + + if (l1s.neigh_pm.n == 0 || !l1s.neigh_sb.running) + goto out; + + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("SB error arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* next sync */ + //if (++l1s.neigh_sb.count == 11) { + if (++l1s.neigh_sb.count == 35) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + } else { + l1s.neigh_sb.count = 0; + + read_sb_result(last_fb, 1); + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + l1s.neigh_sb.flags_bsic[index] = + l1s_decode_sb(&fbs.mon.time, sb) + | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + printf("SB OK!!!!!! arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* store time offset */ + } + +out: + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + return 0; + +} + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(l1s_neigh_sb_cmd, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_neigh_sb_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +const struct tdma_sched_item neigh_sync_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_fb_cmd, 1, 0, 1), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_fb_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + + +static __attribute__ ((constructor)) void l1s_prim_fbsb_init(void) +{ + l1s.completion[L1_COMPL_FB] = &l1a_fb_compl; +} diff --git a/src/target/firmware/layer1/tpu_window.c b/src/target/firmware/layer1/tpu_window.c index f4e76c16..21a3326e 100644 --- a/src/target/firmware/layer1/tpu_window.c +++ b/src/target/firmware/layer1/tpu_window.c @@ -47,7 +47,9 @@ #define L1_NB_DURATION_Q (L1_BURST_LENGTH_Q + 2 * L1_NB_MARGIN_Q - L1_TAIL_DURATION_Q) #define L1_SB_DURATION_Q (L1_BURST_LENGTH_Q + 2 * L1_SB_MARGIN_Q - L1_TAIL_DURATION_Q) #define L1_FB_DURATION_Q (11 * L1_TDMA_LENGTH_Q + 2057) /* more than 11 full slots */ -#define L1_FB26_DURATION_Q (L1_TDMA_LENGTH_Q + 798) +//#define L1_FB26_DURATION_Q (L1_TDMA_LENGTH_Q + 798) +#define L1_FB26_DURATION_Q (2*L1_TDMA_LENGTH_Q + 798) +#define L1_SB26_DURATION_Q (L1_TDMA_LENGTH_Q + 984) // 984 #define L1_PW_DURATION_Q 289 #define DSP_SETUP_TIME 66 @@ -57,6 +59,8 @@ static const uint16_t rx_burst_duration[_NUM_L1_RXWIN] = { [L1_RXWIN_FB] = L1_FB_DURATION_Q, [L1_RXWIN_SB] = L1_SB_DURATION_Q, [L1_RXWIN_NB] = L1_NB_DURATION_Q, + [L1_RXWIN_FB26] = L1_FB26_DURATION_Q, + [L1_RXWIN_SB26] = L1_SB26_DURATION_Q, }; #define L1_TX_NB_DURATION_Q 626 @@ -129,6 +133,15 @@ void l1s_rx_win_ctrl(uint16_t arfcn, enum l1_rxwin_type wtype, uint8_t tn_ofs) stop -= 11 * L1_TDMA_LENGTH_Q; } + /* Delay 11 full TDMA frames */ + if (wtype == L1_RXWIN_FB26) { + uint8_t i; + for (i = 0; i < 2; i++) + tpu_enq_at(0); + + stop -= 2 * L1_TDMA_LENGTH_Q; + } + /* Window close for ABB */ twl3025_downlink(0, stop & 0xffff); diff --git a/src/target/firmware/prim_fbsbwithFB1.c b/src/target/firmware/prim_fbsbwithFB1.c new file mode 100644 index 00000000..c7a17b7f --- /dev/null +++ b/src/target/firmware/prim_fbsbwithFB1.c @@ -0,0 +1,1243 @@ +/* Layer 1 - FCCH and SCH burst handling */ + +/* (C) 2010 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <inttypes.h> +#include <defines.h> +#include <debug.h> +#include <memory.h> +#include <byteorder.h> +#include <rffe.h> +#include <osmocom/gsm/gsm_utils.h> +#include <osmocom/core/msgb.h> +#include <calypso/dsp_api.h> +#include <calypso/irq.h> +#include <calypso/tpu.h> +#include <calypso/tsp.h> +#include <calypso/dsp.h> +#include <calypso/timer.h> +#include <comm/sercomm.h> + +#include <layer1/sync.h> +#include <layer1/afc.h> +#include <layer1/toa.h> +#include <layer1/tdma_sched.h> +#include <layer1/mframe_sched.h> +#include <layer1/tpu_window.h> +#include <layer1/l23_api.h> +#include <layer1/agc.h> + +#include <l1ctl_proto.h> + +#define FB0_RETRY_COUNT 3 +#define AFC_RETRY_COUNT 30 + +extern uint16_t rf_arfcn; // TODO + +struct mon_state { + uint32_t fnr_report; /* frame number when DSP reported it */ + int attempt; /* which attempt was this ? */ + + int16_t toa; + uint16_t pm; + uint16_t angle; + uint16_t snr; + + /* computed values */ + int16_t freq_diff; + + /* Sync Burst (SB) */ + uint8_t bsic; + struct gsm_time time; +}; + +struct l1a_fb_state { + struct mon_state mon; + struct l1ctl_fbsb_req req; + int16_t initial_freq_err; + uint8_t fb_retries; + uint8_t afc_retries; +}; + +static struct l1a_fb_state fbs; +static struct mon_state *last_fb = &fbs.mon; +static int sb_det = 0; //MTZ - This was added +static int fb_det = 0; //MTZ - This was added +uint32_t old_tpu_offset = 0; //MTZ - This was added +int total_sb_det = 0; + +int16_t nb_fb_toa = 0; //MTZ - This was added +uint16_t nb_fb_pm = 0; //MTZ - This was added +uint16_t nb_fb_angle0 = 0; //MTZ - This was added +uint16_t nb_fb_angle1 = 0; //MTZ - This was added +uint16_t nb_fb_snr = 0; //MTZ - This was added + +// MTZ - for working of these variables see comments in l1s_neigh_fbsb_cmd +// MTZ - det_serving_cell overrides neigh_for_fbsb_det +int synchronize_yes = 0; //MTZ - A test variable +int det_serving_cell = 0; //MTZ - A test variable +int neigh_for_fbsb_det = 0; //MTZ - A test variable + +static void dump_mon_state(struct mon_state *fb) +{ +#if 0 + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz, " + "SNR=%04x(%d.%u) OFFSET=%u SYNCHRO=%u\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle), + fb->snr, l1s_snr_int(fb->snr), l1s_snr_fract(fb->snr), + tpu_get_offset(), tpu_get_synchro()); +#else + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle)); +#endif +} + +static int l1ctl_fbsb_resp(uint8_t res) +{ + struct msgb *msg; + struct l1ctl_fbsb_conf *resp; + + msg = l1_create_l2_msg(L1CTL_FBSB_CONF, fbs.mon.time.fn, + l1s_snr_int(fbs.mon.snr), + fbs.req.band_arfcn); + if (!msg) + return -ENOMEM; + + resp = (struct l1ctl_fbsb_conf *) msgb_put(msg, sizeof(*resp)); + resp->initial_freq_err = htons(fbs.initial_freq_err); + resp->result = res; + resp->bsic = fbs.mon.bsic; + + /* no need to set BSIC, as it is never used here */ + l1_queue_for_l2(msg); + + return 0; +} + +/* SCH Burst Detection ********************************************************/ + +/* determine the GSM time and BSIC from a Sync Burst */ +static uint8_t l1s_decode_sb(struct gsm_time *time, uint32_t sb) +{ + uint8_t bsic = (sb >> 2) & 0x3f; + uint8_t t3p; + + memset(time, 0, sizeof(*time)); + + /* TS 05.02 Chapter 3.3.2.2.1 SCH Frame Numbers */ + time->t1 = ((sb >> 23) & 1) | ((sb >> 7) & 0x1fe) | ((sb << 9) & 0x600); + time->t2 = (sb >> 18) & 0x1f; + t3p = ((sb >> 24) & 1) | ((sb >> 15) & 6); + time->t3 = t3p*10 + 1; + + /* TS 05.02 Chapter 4.3.3 TDMA frame number */ + time->fn = gsm_gsmtime2fn(time); + + time->tc = (time->fn / 51) % 8; + + return bsic; +} + +static void read_sb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.db_r->a_serv_demod[D_TOA]; + st->pm = dsp_api.db_r->a_serv_demod[D_PM]>>3; + st->angle = dsp_api.db_r->a_serv_demod[D_ANGLE]; + st->snr = dsp_api.db_r->a_serv_demod[D_SNR]; + + st->freq_diff = ANGLE_TO_FREQ(st->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + if (st->snr > AFC_SNR_THRESHOLD) + afc_input(st->freq_diff, rf_arfcn, 1); + else + afc_input(st->freq_diff, rf_arfcn, 0); + + dsp_api.r_page_used = 1; +} + +/* Note: When we get the SB response, it is 2 TDMA frames after the SB + * actually happened, as it is a "C W W R" task */ +#define SB2_LATENCY 2 + +static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, + __unused uint16_t p3) +{ + uint32_t sb; + int qbits, fn_offset; + struct l1_cell_info *cinfo = &l1s.serving_cell; + int fnr_delta, bits_delta; + + putchart('s'); + + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + /* mark READ page as being used */ + dsp_api.r_page_used = 1; + + /* after 2nd attempt, we failed */ + if (attempt == 2) { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + /* after 1st attempt, we simply wait for 2nd */ + return 0; + } + + printf("SB%d ", attempt); + read_sb_result(last_fb, attempt); + + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + fbs.mon.bsic = l1s_decode_sb(&fbs.mon.time, sb); + printf("=> SB 0x%08"PRIx32": BSIC=%u ", sb, fbs.mon.bsic); + l1s_time_dump(&fbs.mon.time); + + l1s.serving_cell.bsic = fbs.mon.bsic; + + /* calculate synchronisation value (TODO: only complete for qbits) */ + last_fb->toa -= 23; + qbits = last_fb->toa * 4; + fn_offset = l1s.current_time.fn; // TODO + + if (qbits > QBITS_PER_TDMA) { + qbits -= QBITS_PER_TDMA; + fn_offset -= 1; + } else if (qbits < 0) { + qbits += QBITS_PER_TDMA; + fn_offset += 1; + } + + fnr_delta = last_fb->fnr_report - attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports SB in bit that is %d bits in the " + "future?!?\n", last_fb->toa - bits_delta); + else + printf(" qbits=%u\n", qbits); + + synchronize_tdma(&l1s.serving_cell); + + /* if we have recived a SYNC burst, update our local GSM time */ + gsm_fn2gsmtime(&l1s.current_time, fbs.mon.time.fn + SB2_LATENCY); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + /* If we call tdma_sched_reset(), which is only needed if there + * are further l1s_sbdet_resp() scheduled, we will bring + * dsp_api.db_r and dsp_api.db_w out of sync because we changed + * dsp_api.db_w for l1s_sbdet_cmd() and canceled + * l1s_sbdet_resp() which would change dsp_api.db_r. The DSP + * however expects dsp_api.db_w and dsp_api.db_r to be in sync + * (either "0 - 0" or "1 - 1"). So we have to bring dsp_api.db_w + * and dsp_api.db_r into sync again, otherwise NB reading will + * complain. We probably don't need the Abort command and could + * just bring dsp_api.db_w and dsp_api.db_r into sync. */ + if (attempt != 2) { + tdma_sched_reset(); + l1s_dsp_abort(); + } + + l1s_reset_hw(); + /* enable the MF Task for BCCH reading */ + mframe_enable(MF_TASK_BCCH_NORM); + printf("\nMTZ: l1s.serving_cell.ccch_mode = %d\n", l1s.serving_cell.ccch_mode); + if (l1s.serving_cell.ccch_mode == CCCH_MODE_COMBINED) + mframe_enable(MF_TASK_CCCH_COMB); + else if (l1s.serving_cell.ccch_mode == CCCH_MODE_NON_COMBINED) + mframe_enable(MF_TASK_CCCH); + + l1s_compl_sched(L1_COMPL_FB); + + return 0; +} + +static int l1s_sbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + putchart('S'); + + fbs.mon.bsic = 0; + fbs.mon.time.fn = 0; + + dsp_api.db_w->d_task_md = SB_DSP_TASK; + dsp_api.ndb->d_fb_mode = 0; /* wideband search */ + + /* Program TPU */ + printf("\nMTZ: arfcn in l1s_sbdet_cmd = %d\n", rf_arfcn); + l1s_rx_win_ctrl(rf_arfcn, L1_RXWIN_SB, 0); + + return 0; +} + +/* This is how it is done by the TSM30 */ +static const struct tdma_sched_item sb_sched_set[] = { + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 2), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +void l1s_sb_test(uint8_t base_fn) +{ + tdma_schedule_set(base_fn, sb_sched_set, 0); +} +/* FCCH Burst *****************************************************************/ + +static int read_fb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.ndb->a_sync_demod[D_TOA]; + st->pm = dsp_api.ndb->a_sync_demod[D_PM]>>3; + st->angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + st->snr = dsp_api.ndb->a_sync_demod[D_SNR]; + + //last_fb->angle = clip_int16(last_fb->angle, AFC_MAX_ANGLE); + st->freq_diff = ANGLE_TO_FREQ(last_fb->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + dsp_api.ndb->d_fb_det = 0; + dsp_api.ndb->a_sync_demod[D_TOA] = 0; /* TSM30 does it (really needed ?) */ + + /* Update AFC with current frequency offset */ + afc_correct(st->freq_diff, rf_arfcn); + + //tpu_dsp_frameirq_enable(); + return 1; +} + +static void fbinfo2cellinfo(struct l1_cell_info *cinfo, + const struct mon_state *mon) +{ + int ntdma, qbits, fn_offset, fnr_delta, bits_delta; + + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + fnr_delta = last_fb->fnr_report - last_fb->attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports FB in bit that is %d bits in " + "the future?!?\n", last_fb->toa - bits_delta); + else { + int fb_fnr = (last_fb->fnr_report - last_fb->attempt) + + last_fb->toa/BITS_PER_TDMA; + printf("=>FB @ FNR %u fn_offset=%d qbits=%u\n", + fb_fnr, fn_offset, qbits); + } +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_fbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + if (fb_mode == 0) { + putchart('F'); + } else { + putchart('V'); + } + + l1s.fb.mode = fb_mode; + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = FB_DSP_TASK; /* maybe with I/Q swap? */ + dsp_api.ndb->d_fb_mode = fb_mode; + + /* Program TPU */ + printf("\nMTZ: arfcn in l1s_fbdet_cmd = %d\n", fbs.req.band_arfcn); + l1s_rx_win_ctrl(fbs.req.band_arfcn, L1_RXWIN_FB, 0); + + return 0; +} + +#if 0 +#define FB0_SNR_THRESH 2000 +#define FB1_SNR_THRESH 3000 +#else +#define FB0_SNR_THRESH 0 +#define FB1_SNR_THRESH 0 +#endif + +static const struct tdma_sched_item fb_sched_set[]; + +/* scheduler callback to check for a FB detection response */ +static int l1s_fbdet_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + putchart('f'); + + if (!dsp_api.ndb->d_fb_det) { + /* we did not detect a FB */ + + /* attempt < 12, do nothing */ + if (attempt < 12) + return 0; + + /* attempt >= 12, we simply don't find one */ + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + if (fbs.fb_retries < FB0_RETRY_COUNT) { + /* retry once more */ + tdma_schedule_set(1, fb_sched_set, 0); + fbs.fb_retries++; + } else { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; + } + + /* We found a frequency burst, reset everything */ + l1s_reset_hw(); + + printf("FB%u ", dsp_api.ndb->d_fb_mode); + read_fb_result(last_fb, attempt); + + /* if this is the first success, save freq err */ + if (!fbs.initial_freq_err) + fbs.initial_freq_err = last_fb->freq_diff; + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + /* Immediately schedule further TDMA tasklets, if requested. Doing + * this directly from L1S means we can do this quickly without any + * additional delays */ + if (fb_mode == 0) { + if (fbs.req.flags & L1CTL_FBSB_F_FB1) { + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + /* FIXME: don't only use the last but an average */ + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh1 && + last_fb->snr > FB0_SNR_THRESH) { + /* continue with FB1 task in DSP */ + tdma_schedule_set(1, fb_sched_set, 1); + } else { + if (fbs.afc_retries < AFC_RETRY_COUNT) { + tdma_schedule_set(1, fb_sched_set, 0); + fbs.afc_retries++; + } else { + /* Abort */ + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + } + } else + l1s_compl_sched(L1_COMPL_FB); + } else if (fb_mode == 1) { + if (fbs.req.flags & L1CTL_FBSB_F_SB) { + + int ntdma, qbits; + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + + int fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + int delay = fn_offset + 11 - l1s.current_time.fn - 1; + printf(" fn_offset=%d (fn=%"PRIu32" + attempt=%u + ntdma = %d)\n", + fn_offset, l1s.current_time.fn, last_fb->attempt, ntdma); + printf(" delay=%d (fn_offset=%d + 11 - fn=%"PRIu32" - 1\n", delay, + fn_offset, l1s.current_time.fn); + printf(" scheduling next FB/SB detection task with delay %u\n", delay); + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh2 && + last_fb->snr > FB1_SNR_THRESH) { + /* synchronize before reading SB */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + synchronize_tdma(&l1s.serving_cell); + tdma_schedule_set(delay, sb_sched_set, 0); + } else + tdma_schedule_set(delay, fb_sched_set, 1); + } else + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; +} + +/* FB detection */ +static const struct tdma_sched_item fb_sched_set[] = { + SCHED_ITEM_DT(l1s_fbdet_cmd, 0, 0, 0), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 3), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 4), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 5), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 6), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 7), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 8), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 9), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 10), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 11), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 12), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +/* Asynchronous completion handler for FB detection */ +static void l1a_fb_compl(__unused enum l1_compl c) +{ + if (last_fb->attempt >= 13) { + /* FB detection failed, signal this via L1CTL */ + l1ctl_fbsb_resp(255); + return; + } + + /* FIME: use l1s.neigh_cell[fbs.cinfo_idx] */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + + /* send FBSB_CONF success message via L1CTL */ + l1ctl_fbsb_resp(0); +} + +void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req) +{ + /* copy + endian convert request data */ + fbs.req.band_arfcn = ntohs(req->band_arfcn); + fbs.req.timeout = ntohs(req->timeout); + fbs.req.freq_err_thresh1 = ntohs(req->freq_err_thresh1); + fbs.req.freq_err_thresh2 = ntohs(req->freq_err_thresh2); + fbs.req.num_freqerr_avg = req->num_freqerr_avg; + fbs.req.flags = req->flags; + fbs.req.sync_info_idx = req->sync_info_idx; + fbs.req.rxlev_exp = req->rxlev_exp; + + /* clear initial frequency error */ + fbs.initial_freq_err = 0; + fbs.fb_retries = 0; + fbs.afc_retries = 0; + + /* Make sure we start at a 'center' AFCDAC output value */ + afc_reset(); + + /* Reset the TOA loop counters */ + toa_reset(); + + if (fbs.req.flags & L1CTL_FBSB_F_FB0) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_FB1) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_SB) + tdma_schedule_set(base_fn, sb_sched_set, 0); + +} + +/* SB for Neighbours in dedicated mode + * + * Only when number of neighbor cells is > 0, perform synchronization. + * + * For each synchronization, l1s.neigh_pm.running is set. In case of an update + * of neighbor cell list, this state is cleared, so a pending sync result would + * be ignored. + * + * After a (new) list of neighbor cells are received, the measurements are not + * yet valid. A valid state flag is used to indicate valid measurements. Until + * there are no valid measurements, the synchronization is not performed. + * + * The task is to scan the 6 strongest neighbor cells by trying to synchronize + * to it. This is done by selecting the strongest unscanned neighbor cell. + * If 6 cells have been scanned or all cells (if less than 6) have been + * scanned, the process clears all 'scanned' flags and starts over with the + * strongest (now the strongest unscanned) cell. + * + * Each synchronization attempt is performed during the "search frame" (IDLE + * frame). The process attempts to sync 11 times to ensure that it hits the + * SCH of the neighbor's BCCH. (The alignment of BCCH shifts after every two + * 26-multiframe in a way that the "search frame" is aligned with the SCH, at + * least once for 11 successive "search frames".) + * + * If the synchronization attempt is successful, the BSIC and neighbor cell + * offset is stored. These are indicated to layer23 with the measurement + * results. + * + * When performing handover to a neighbor cell, the stored offset is used to + * calculate new GSM time and tpu_offset. + */ + +static void select_neigh_cell(void) +{ + uint8_t strongest = 0, strongest_unscanned = 0; + int strongest_i = 0, strongest_unscanned_i = -1; + int num_scanned = 0; + int i; + + /* find strongest cell and strongest unscanned cell and count */ + for (i = 0; i < l1s.neigh_pm.n; i++) { + if (l1s.neigh_pm.level[i] > strongest) { + strongest = l1s.neigh_pm.level[i]; + strongest_i = i; + } + if (!(l1s.neigh_sb.flags_bsic[i] & NEIGH_PM_FLAG_SCANNED)) { + if (l1s.neigh_pm.level[i] > strongest_unscanned) { + strongest_unscanned = l1s.neigh_pm.level[i]; + strongest_unscanned_i = i; + } + } else + num_scanned++; + } + + /* no unscanned cell or we have scanned enough */ + if (strongest_unscanned_i < 0 || num_scanned >= 6) { + /* flag all cells unscanned */ + for (i = 0; i < l1s.neigh_pm.n; i++) + l1s.neigh_sb.flags_bsic[i] &= ~NEIGH_PM_FLAG_SCANNED; + /* use strongest cell to begin scanning with */ + l1s.neigh_sb.index = strongest_i; + } else { + /* use strongest unscanned cell to begin scanning with */ + l1s.neigh_sb.index = strongest_unscanned_i; + } +} + +//MTZ - The function below has been taken from synchronize_tdma in sync.c and modified for whatever seemed necessary +void synchronize_tdma2() +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int sync_test1(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + printf("\n\nMTZ - sync_test1, old_tpu_offset = %d\n\n", l1s.tpu_offset); + + old_tpu_offset = l1s.tpu_offset; + + //l1s.tpu_offset = 3000; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(l1s.tpu_offset); + //tpu_enq_at(0); + //tpu_enq_at(0); + //l1s.tpu_offset = old_tpu_offset; + //l1s.tpu_offset = 2000; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(l1s.tpu_offset); + + //tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(3000); + //tpu_enq_at(0); + //tpu_enq_at(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(old_tpu_offset); + //tpu_enq_at(0); + + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(2000); + ////tpu_enq_at(0); + ////tpu_enq_at(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(old_tpu_offset); + + printf("\n\nMTZ - sync_test1, ending tpu_offset = %d\n\n", l1s.tpu_offset); + + +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int sync_test2(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + printf("\n\nMTZ - sync_test2\n\n"); + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_sync(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + if (fb_det == 1) { + afc_correct(ANGLE_TO_FREQ(nb_fb_angle0), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + } + + if ((sb_det == 2)||(sb_det == 4)) { + //printf("\nMTZ - in l1s_neigh_fbsb_sync, old_tpu_offset = %d\n", l1s.tpu_offset); + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; //MTZ - uncomment + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; //MTZ - uncomment + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + //printf("MTZ - old_tpu_offset = %d, tpu_shift = %d, qbits = %d\n", old_tpu_offset, tpu_shift, qbits); + + l1s.neigh_pm.tpu_offset[l1s.neigh_sb.index] = tpu_shift; + + int ii =0; + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]) { + l1s.tpu_offsets[ii] = tpu_shift; + break; + } + if (l1s.tpu_offsets_arfcn[ii] == 0) { + l1s.tpu_offsets_arfcn[ii] = l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]; + l1s.tpu_offsets[ii] = tpu_shift; + break; + } + } + + printf("\n\nMTZ: Stored TPU Offsets:"); + for (ii=0; ii<64; ii++) { + if (l1s.tpu_offsets_arfcn[ii] == 0) + break; + printf(" %d(%d)", l1s.tpu_offsets[ii], l1s.tpu_offsets_arfcn[ii]); + } + printf("\n\n"); + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50) { + l1s.tpu_offset = tpu_shift; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(tpu_shift); + } + afc_correct(ANGLE_TO_FREQ(nb_fb_angle1), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + + if (synchronize_yes) { + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + +} +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + //printf("\nMTZ: In neigh_fbsb_cmd, l1s.neigh_pm.n = %d", l1s.neigh_pm.n); + + //---------------------------------------------------------------------------------------- + // + // Important points of note: + // ------------------------- + // + // Temporary variables used for testing: + // + // Right now we have three global variables defined at the beginning of this file + // for testing purposes: det_serving_cell, synchronize_yes and neigh_for_fbsb_det. + // + // det_serving_cell allows for detection of FBSB on serving cell, something which we + // thought should be simpler as the clock is already synched on that cell. + // + // synchronize_yes is a variable with which one can control whether offset and synch + // commands will be sent to the TPU. There was a thought that perhaps the DSP SB + // detection task for dedicated mode might not require synching due to which this + // test variable was created. Also, naturally, detection on the serving cell should + // not require synching but we could be wrong. + // + // neigh_for_fbsb_det is a variable to hardcode the neighbour one wants to detect + // FBSB on. Note that because more functionality is added to the code the mechanism + // for traversing through neighbours using select_neigh_cell and certain flags is + // disturbed for now. Due to this the index is hardcoded in the code below to + // whichever neighbour one wants to detect FBSB on (ordered by signal strength) + // using this variable. + // + // Progress so far: + // + // Right now we are able to detect FB in dedicated mode even for neighbours. One + // additional thing we have done to aid us is that we have added 3 more frames to + // our idle frame by taking up some TCH channels (this can be seen in mframe_sched). + // The call doesn't get dropped if synchronization is not performed except for very + // few cases which we need not worry about for now. However, when synchronization + // is performed the call gets dropped without exception. + // + // Few points where the problem could lie: + // + // 1) Perhaps we are not using the TCH_SB_DSP_TASK correctly. Perhaps some other + // locations in the API also need to be written to when issuing the command in + // addition to dsp_api.db_w->d_task_md. Perhaps we are not reading from the correct + // location when checking the response. + // + // 2) Perhaps we need to start the SB detection task some quarter-bits or timeslots + // before the actual SB appears. + // + // 3) Timing issues. This is the most likely cause as we haven't really been able + // to fully understand and implement timing (if it is required, which it seems like + // it is) in the code. If timing is required then this code is quite superficial. + // In idle mode functions such as afc_correct and procedures requiring frequency + // difference are used to before calling SB detection task which are not done here. + // + // Timing issues seems to be the most likely problem. Anyone wishing to solve the + // SB detection issue should try to focus on this problem, understand how + // synchronization is performed in idle mode and how we can do that in dedicated + // mode and be back within 4 frames as after that the traffic needs to resume. + // + //---------------------------------------------------------------------------------------- + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + index = l1s.neigh_sb.index; + } + +// //MTZ - putting this for now as we wanted to repeatedly detect the remaining ones - remove +// while (!((l1s.neigh_sb.flags_bsic[index] & NEIGH_PM_FLAG_BSIC) == 0)) { +//// printf("\nMTZ: BSIC has been decoded for ARFCN %d (flags_bsic[%d] = %d)\n\n", l1s.neigh_pm.band_arfcn[index], index, l1s.neigh_sb.flags_bsic[index]); +// l1s.neigh_sb.count = 0; +// l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; +// select_neigh_cell(); +// index = l1s.neigh_sb.index; +// } + + //MTZ - This index variable is used to hardcode the neighbour cell for now + //index = neigh_for_fbsb_det; + + if (sb_det == 0) { + + //l1s.fb.mode = fb_mode; + +// if (det_serving_cell) +// printf(" - detect FB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); +// else +// printf(" - detect FB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_FB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + if (fb_det == 1) { + dsp_api.ndb->d_fb_mode = 1; + } else { + dsp_api.ndb->d_fb_mode = 0; + } + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 5); //MTZ - Original - don't think works + printf("\nMTZ: arfcn in l1s_neigh_fbsb_cmd (FB%d) = %d\n", fb_det, l1s.neigh_pm.band_arfcn[index]); + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_FB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + } else if ((sb_det == 2)||(sb_det == 4)) { + +// if (det_serving_cell) +// printf(" - detect SB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); +// else +// printf(" - detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + //MTZ - This is a variable for the testing phase whether to send sync commands or not + //if (synchronize_yes) + // synchronize_tdma2(); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + +// //MTZ - Experimenting +// dsp_api.ndb->a_sync_demod[D_TOA] = nb_fb_toa; +// dsp_api.ndb->a_sync_demod[D_PM] = nb_fb_pm; +// dsp_api.ndb->a_sync_demod[D_ANGLE] = nb_fb_angle; +// dsp_api.ndb->a_sync_demod[D_SNR] = nb_fb_snr; + + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); //MTZ - Original - don't think works + printf("\nMTZ: arfcn in l1s_neigh_fbsb_cmd (SB) = %d\n", l1s.neigh_pm.band_arfcn[index]); + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_SB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + } + return 0; +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + uint8_t bsic; + int sb_found = 0; + + if (sb_det == 0) { + if (!dsp_api.ndb->d_fb_det) { + printf("MTZ: ARFCN %d (index %d, power %d dbm, try #%d) FB%d found = 0\n", l1s.neigh_pm.band_arfcn[index], index, rxlev2dbm(l1s.neigh_pm.level[index]), l1s.neigh_sb.count, fb_det); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + if (fb_det == 1){ + afc_correct(-1*ANGLE_TO_FREQ(nb_fb_angle0), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + fb_det = 0; + } + } + + } else { + nb_fb_toa = dsp_api.ndb->a_sync_demod[D_TOA]; + nb_fb_pm = dsp_api.ndb->a_sync_demod[D_PM]; + if (fb_det == 1) + nb_fb_angle1 = dsp_api.ndb->a_sync_demod[D_ANGLE]; + else + nb_fb_angle0 = dsp_api.ndb->a_sync_demod[D_ANGLE]; + nb_fb_snr = dsp_api.ndb->a_sync_demod[D_SNR]; + printf("\n\nMTZ: ARFCN %d (index %d, power %d dbm, try #%d) FB%d found = 1 >>> nb_fb_toa = %d\n\n", l1s.neigh_pm.band_arfcn[index], index, rxlev2dbm(l1s.neigh_pm.level[index]), l1s.neigh_sb.count, fb_det, nb_fb_toa); + if (fb_det == 0) { + fb_det = 1; + } else { + sb_det = 1; + fb_det = 0; + } + } + + //l1s_reset_hw(); + tdma_sched_reset(); + } else { + + if ((sb_det == 2)||(sb_det == 4)) { + /* check if sync was successful */ + //if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + // printf("\nSB found1 = 0\n"); + //} else { + // printf("\nSB found1 = 1\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + //} + + if (dsp_api.ndb->a_sch26[0] & (1<<B_SCH_CRC)) { +// printf("\nSB found = 0 (ARFCN %d, power %d dbm)\n\n", l1s.neigh_pm.band_arfcn[index], rxlev2dbm(l1s.neigh_pm.level[index])); + } else { + sb_found = 1; + sb = dsp_api.ndb->a_sch26[3] | dsp_api.ndb->a_sch26[4] << 16; + bsic = (sb >> 2) & 0x3f; + total_sb_det++; + //printf("=> SB 0x%08"PRIx32": BSIC=%u \n\n", sb, bsic); + printf("\n----------------------------------------------------------------------------\nSB found = 1 (ARFCN %d, power %d dbm) => SB 0x%08"PRIx32": BSIC=%u, TOA=%d (Total=%d)\n----------------------------------------------------------------------------\n\n", l1s.neigh_pm.band_arfcn[index], rxlev2dbm(l1s.neigh_pm.level[index]), sb, bsic, dsp_api.db_r->a_serv_demod[D_TOA], total_sb_det); + l1s.neigh_sb.flags_bsic[index] = bsic | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + if ((l1s.new_dm == 1) && (l1s.neigh_pm.band_arfcn[index] != 58)) { + l1s.ho_arfcn = l1s.neigh_pm.band_arfcn[index]; + l1s.new_dm = 0; + } + } + + if ((sb_det == 2)||(sb_det == 4)) { + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50); + l1s.tpu_offset = old_tpu_offset; + afc_correct(-1*(ANGLE_TO_FREQ(nb_fb_angle0) + ANGLE_TO_FREQ(nb_fb_angle1)), l1s.neigh_pm.band_arfcn[l1s.neigh_sb.index]); + + if (synchronize_yes) { + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + + if ((sb_det == 4)||(sb_found == 1)) { + l1s.neigh_sb.count = 0; + //MTZ - need to change this statement based on detection + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + if (sb_found == 0) + printf("\n\n"); + } + + } + + if ((sb_det == 4)||(sb_found == 1)) + sb_det = 0; + else + sb_det++; + } + + return 0; +} + +static int l1s_neigh_sb_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\n\n\nMTZ: In neigh_sb_cmd, l1s.neigh_pm.n = %d\n\n\n", l1s.neigh_pm.n); + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + return 0; +} + +static int l1s_neigh_sb_resp(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + + if (l1s.neigh_pm.n == 0 || !l1s.neigh_sb.running) + goto out; + + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("SB error arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + } else { + l1s.neigh_sb.count = 0; + + read_sb_result(last_fb, 1); + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + l1s.neigh_sb.flags_bsic[index] = + l1s_decode_sb(&fbs.mon.time, sb) + | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + printf("SB OK!!!!!! arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* store time offset */ + } + +out: + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + printf("\nMTZ: In l1s_neigh_sb_resp, l1s.serving_cell.arfcn = %d\n", l1s.serving_cell.arfcn); + + return 0; + +} + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(l1s_neigh_sb_cmd, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_neigh_sb_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +const struct tdma_sched_item neigh_sync_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_fbsb_sync, 1, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_neigh_fbsb_cmd, 1, 0, 1), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_fbsb_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(sync_test1, 1, 0, 1), SCHED_END_FRAME(), +//// SCHED_END_FRAME(), +//// SCHED_END_FRAME(), +//// SCHED_ITEM_DT(sync_test2, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + + +static __attribute__ ((constructor)) void l1s_prim_fbsb_init(void) +{ + l1s.completion[L1_COMPL_FB] = &l1a_fb_compl; +} diff --git a/src/target/firmware/prim_fbsbwithTotal.c b/src/target/firmware/prim_fbsbwithTotal.c new file mode 100644 index 00000000..b325da47 --- /dev/null +++ b/src/target/firmware/prim_fbsbwithTotal.c @@ -0,0 +1,1188 @@ +/* Layer 1 - FCCH and SCH burst handling */ + +/* (C) 2010 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <inttypes.h> +#include <defines.h> +#include <debug.h> +#include <memory.h> +#include <byteorder.h> +#include <rffe.h> +#include <osmocom/gsm/gsm_utils.h> +#include <osmocom/core/msgb.h> +#include <calypso/dsp_api.h> +#include <calypso/irq.h> +#include <calypso/tpu.h> +#include <calypso/tsp.h> +#include <calypso/dsp.h> +#include <calypso/timer.h> +#include <comm/sercomm.h> + +#include <layer1/sync.h> +#include <layer1/afc.h> +#include <layer1/toa.h> +#include <layer1/tdma_sched.h> +#include <layer1/mframe_sched.h> +#include <layer1/tpu_window.h> +#include <layer1/l23_api.h> +#include <layer1/agc.h> + +#include <l1ctl_proto.h> + +#define FB0_RETRY_COUNT 3 +#define AFC_RETRY_COUNT 30 + +extern uint16_t rf_arfcn; // TODO + +struct mon_state { + uint32_t fnr_report; /* frame number when DSP reported it */ + int attempt; /* which attempt was this ? */ + + int16_t toa; + uint16_t pm; + uint16_t angle; + uint16_t snr; + + /* computed values */ + int16_t freq_diff; + + /* Sync Burst (SB) */ + uint8_t bsic; + struct gsm_time time; +}; + +struct l1a_fb_state { + struct mon_state mon; + struct l1ctl_fbsb_req req; + int16_t initial_freq_err; + uint8_t fb_retries; + uint8_t afc_retries; +}; + +static struct l1a_fb_state fbs; +static struct mon_state *last_fb = &fbs.mon; +static int sb_det = 0; //MTZ - This was added +uint32_t old_tpu_offset = 0; //MTZ - This was added +int total_sb_det = 0; + +int16_t nb_fb_toa = 0; //MTZ - This was added +uint16_t nb_fb_pm = 0; //MTZ - This was added +uint16_t nb_fb_angle = 0; //MTZ - This was added +uint16_t nb_fb_snr = 0; //MTZ - This was added + +// MTZ - for working of these variables see comments in l1s_neigh_fbsb_cmd +// MTZ - det_serving_cell overrides neigh_for_fbsb_det +int synchronize_yes = 0; //MTZ - A test variable +int det_serving_cell = 0; //MTZ - A test variable +int neigh_for_fbsb_det = 0; //MTZ - A test variable + +static void dump_mon_state(struct mon_state *fb) +{ +#if 0 + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz, " + "SNR=%04x(%d.%u) OFFSET=%u SYNCHRO=%u\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle), + fb->snr, l1s_snr_int(fb->snr), l1s_snr_fract(fb->snr), + tpu_get_offset(), tpu_get_synchro()); +#else + printf("(%"PRIu32":%u): TOA=%5u, Power=%4ddBm, Angle=%5dHz\n", + fb->fnr_report, fb->attempt, fb->toa, + agc_inp_dbm8_by_pm(fb->pm)/8, ANGLE_TO_FREQ(fb->angle)); +#endif +} + +static int l1ctl_fbsb_resp(uint8_t res) +{ + struct msgb *msg; + struct l1ctl_fbsb_conf *resp; + + msg = l1_create_l2_msg(L1CTL_FBSB_CONF, fbs.mon.time.fn, + l1s_snr_int(fbs.mon.snr), + fbs.req.band_arfcn); + if (!msg) + return -ENOMEM; + + resp = (struct l1ctl_fbsb_conf *) msgb_put(msg, sizeof(*resp)); + resp->initial_freq_err = htons(fbs.initial_freq_err); + resp->result = res; + resp->bsic = fbs.mon.bsic; + + /* no need to set BSIC, as it is never used here */ + l1_queue_for_l2(msg); + + return 0; +} + +/* SCH Burst Detection ********************************************************/ + +/* determine the GSM time and BSIC from a Sync Burst */ +static uint8_t l1s_decode_sb(struct gsm_time *time, uint32_t sb) +{ + uint8_t bsic = (sb >> 2) & 0x3f; + uint8_t t3p; + + memset(time, 0, sizeof(*time)); + + /* TS 05.02 Chapter 3.3.2.2.1 SCH Frame Numbers */ + time->t1 = ((sb >> 23) & 1) | ((sb >> 7) & 0x1fe) | ((sb << 9) & 0x600); + time->t2 = (sb >> 18) & 0x1f; + t3p = ((sb >> 24) & 1) | ((sb >> 15) & 6); + time->t3 = t3p*10 + 1; + + /* TS 05.02 Chapter 4.3.3 TDMA frame number */ + time->fn = gsm_gsmtime2fn(time); + + time->tc = (time->fn / 51) % 8; + + return bsic; +} + +static void read_sb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.db_r->a_serv_demod[D_TOA]; + st->pm = dsp_api.db_r->a_serv_demod[D_PM]>>3; + st->angle = dsp_api.db_r->a_serv_demod[D_ANGLE]; + st->snr = dsp_api.db_r->a_serv_demod[D_SNR]; + + st->freq_diff = ANGLE_TO_FREQ(st->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + if (st->snr > AFC_SNR_THRESHOLD) + afc_input(st->freq_diff, rf_arfcn, 1); + else + afc_input(st->freq_diff, rf_arfcn, 0); + + dsp_api.r_page_used = 1; +} + +/* Note: When we get the SB response, it is 2 TDMA frames after the SB + * actually happened, as it is a "C W W R" task */ +#define SB2_LATENCY 2 + +static int l1s_sbdet_resp(__unused uint8_t p1, uint8_t attempt, + __unused uint16_t p3) +{ + uint32_t sb; + int qbits, fn_offset; + struct l1_cell_info *cinfo = &l1s.serving_cell; + int fnr_delta, bits_delta; + + putchart('s'); + + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + /* mark READ page as being used */ + dsp_api.r_page_used = 1; + + /* after 2nd attempt, we failed */ + if (attempt == 2) { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + /* after 1st attempt, we simply wait for 2nd */ + return 0; + } + + printf("SB%d ", attempt); + read_sb_result(last_fb, attempt); + + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + fbs.mon.bsic = l1s_decode_sb(&fbs.mon.time, sb); + printf("=> SB 0x%08"PRIx32": BSIC=%u ", sb, fbs.mon.bsic); + l1s_time_dump(&fbs.mon.time); + + l1s.serving_cell.bsic = fbs.mon.bsic; + + /* calculate synchronisation value (TODO: only complete for qbits) */ + last_fb->toa -= 23; + qbits = last_fb->toa * 4; + fn_offset = l1s.current_time.fn; // TODO + + if (qbits > QBITS_PER_TDMA) { + qbits -= QBITS_PER_TDMA; + fn_offset -= 1; + } else if (qbits < 0) { + qbits += QBITS_PER_TDMA; + fn_offset += 1; + } + + fnr_delta = last_fb->fnr_report - attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports SB in bit that is %d bits in the " + "future?!?\n", last_fb->toa - bits_delta); + else + printf(" qbits=%u\n", qbits); + + synchronize_tdma(&l1s.serving_cell); + + /* if we have recived a SYNC burst, update our local GSM time */ + gsm_fn2gsmtime(&l1s.current_time, fbs.mon.time.fn + SB2_LATENCY); + /* compute next time from new current time */ + l1s.next_time = l1s.current_time; + l1s_time_inc(&l1s.next_time, 1); + + /* If we call tdma_sched_reset(), which is only needed if there + * are further l1s_sbdet_resp() scheduled, we will bring + * dsp_api.db_r and dsp_api.db_w out of sync because we changed + * dsp_api.db_w for l1s_sbdet_cmd() and canceled + * l1s_sbdet_resp() which would change dsp_api.db_r. The DSP + * however expects dsp_api.db_w and dsp_api.db_r to be in sync + * (either "0 - 0" or "1 - 1"). So we have to bring dsp_api.db_w + * and dsp_api.db_r into sync again, otherwise NB reading will + * complain. We probably don't need the Abort command and could + * just bring dsp_api.db_w and dsp_api.db_r into sync. */ + if (attempt != 2) { + tdma_sched_reset(); + l1s_dsp_abort(); + } + + l1s_reset_hw(); + /* enable the MF Task for BCCH reading */ + mframe_enable(MF_TASK_BCCH_NORM); + if (l1s.serving_cell.ccch_mode == CCCH_MODE_COMBINED) + mframe_enable(MF_TASK_CCCH_COMB); + else if (l1s.serving_cell.ccch_mode == CCCH_MODE_NON_COMBINED) + mframe_enable(MF_TASK_CCCH); + + l1s_compl_sched(L1_COMPL_FB); + + return 0; +} + +static int l1s_sbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + putchart('S'); + + fbs.mon.bsic = 0; + fbs.mon.time.fn = 0; + + dsp_api.db_w->d_task_md = SB_DSP_TASK; + dsp_api.ndb->d_fb_mode = 0; /* wideband search */ + + /* Program TPU */ + l1s_rx_win_ctrl(rf_arfcn, L1_RXWIN_SB, 0); + + return 0; +} + +/* This is how it is done by the TSM30 */ +static const struct tdma_sched_item sb_sched_set[] = { + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_sbdet_cmd, 0, 0, 2), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_sbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +void l1s_sb_test(uint8_t base_fn) +{ + tdma_schedule_set(base_fn, sb_sched_set, 0); +} +/* FCCH Burst *****************************************************************/ + +static int read_fb_result(struct mon_state *st, int attempt) +{ + st->toa = dsp_api.ndb->a_sync_demod[D_TOA]; + st->pm = dsp_api.ndb->a_sync_demod[D_PM]>>3; + st->angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + st->snr = dsp_api.ndb->a_sync_demod[D_SNR]; + + //last_fb->angle = clip_int16(last_fb->angle, AFC_MAX_ANGLE); + st->freq_diff = ANGLE_TO_FREQ(last_fb->angle); + st->fnr_report = l1s.current_time.fn; + st->attempt = attempt; + + dump_mon_state(st); + + dsp_api.ndb->d_fb_det = 0; + dsp_api.ndb->a_sync_demod[D_TOA] = 0; /* TSM30 does it (really needed ?) */ + + /* Update AFC with current frequency offset */ + afc_correct(st->freq_diff, rf_arfcn); + + //tpu_dsp_frameirq_enable(); + return 1; +} + +static void fbinfo2cellinfo(struct l1_cell_info *cinfo, + const struct mon_state *mon) +{ + int ntdma, qbits, fn_offset, fnr_delta, bits_delta; + + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + fnr_delta = last_fb->fnr_report - last_fb->attempt; + bits_delta = fnr_delta * BITS_PER_TDMA; + + cinfo->fn_offset = fnr_delta; + cinfo->time_alignment = qbits; + cinfo->arfcn = rf_arfcn; + + if (last_fb->toa > bits_delta) + printf("=> DSP reports FB in bit that is %d bits in " + "the future?!?\n", last_fb->toa - bits_delta); + else { + int fb_fnr = (last_fb->fnr_report - last_fb->attempt) + + last_fb->toa/BITS_PER_TDMA; + printf("=>FB @ FNR %u fn_offset=%d qbits=%u\n", + fb_fnr, fn_offset, qbits); + } +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_fbdet_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + if (fb_mode == 0) { + putchart('F'); + } else { + putchart('V'); + } + + l1s.fb.mode = fb_mode; + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = FB_DSP_TASK; /* maybe with I/Q swap? */ + dsp_api.ndb->d_fb_mode = fb_mode; + + /* Program TPU */ + l1s_rx_win_ctrl(fbs.req.band_arfcn, L1_RXWIN_FB, 0); + + return 0; +} + +#if 0 +#define FB0_SNR_THRESH 2000 +#define FB1_SNR_THRESH 3000 +#else +#define FB0_SNR_THRESH 0 +#define FB1_SNR_THRESH 0 +#endif + +static const struct tdma_sched_item fb_sched_set[]; + +/* scheduler callback to check for a FB detection response */ +static int l1s_fbdet_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + putchart('f'); + + if (!dsp_api.ndb->d_fb_det) { + /* we did not detect a FB */ + + /* attempt < 12, do nothing */ + if (attempt < 12) + return 0; + + /* attempt >= 12, we simply don't find one */ + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + if (fbs.fb_retries < FB0_RETRY_COUNT) { + /* retry once more */ + tdma_schedule_set(1, fb_sched_set, 0); + fbs.fb_retries++; + } else { + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; + } + + /* We found a frequency burst, reset everything */ + l1s_reset_hw(); + + printf("FB%u ", dsp_api.ndb->d_fb_mode); + read_fb_result(last_fb, attempt); + + /* if this is the first success, save freq err */ + if (!fbs.initial_freq_err) + fbs.initial_freq_err = last_fb->freq_diff; + + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + + /* Immediately schedule further TDMA tasklets, if requested. Doing + * this directly from L1S means we can do this quickly without any + * additional delays */ + if (fb_mode == 0) { + if (fbs.req.flags & L1CTL_FBSB_F_FB1) { + /* If we don't reset here, we get DSP DMA errors */ + tdma_sched_reset(); + /* FIXME: don't only use the last but an average */ + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh1 && + last_fb->snr > FB0_SNR_THRESH) { + /* continue with FB1 task in DSP */ + tdma_schedule_set(1, fb_sched_set, 1); + } else { + if (fbs.afc_retries < AFC_RETRY_COUNT) { + tdma_schedule_set(1, fb_sched_set, 0); + fbs.afc_retries++; + } else { + /* Abort */ + last_fb->attempt = 13; + l1s_compl_sched(L1_COMPL_FB); + } + } + } else + l1s_compl_sched(L1_COMPL_FB); + } else if (fb_mode == 1) { + if (fbs.req.flags & L1CTL_FBSB_F_SB) { + + int ntdma, qbits; + /* FIXME: where did this magic 23 come from? */ + last_fb->toa -= 23; + + if (last_fb->toa < 0) { + qbits = (last_fb->toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (last_fb->toa) / BITS_PER_TDMA; + qbits = (last_fb->toa - ntdma * BITS_PER_TDMA) * 4; + } + + + int fn_offset = l1s.current_time.fn - last_fb->attempt + ntdma; + int delay = fn_offset + 11 - l1s.current_time.fn - 1; + printf(" fn_offset=%d (fn=%"PRIu32" + attempt=%u + ntdma = %d)\n", + fn_offset, l1s.current_time.fn, last_fb->attempt, ntdma); + printf(" delay=%d (fn_offset=%d + 11 - fn=%"PRIu32" - 1\n", delay, + fn_offset, l1s.current_time.fn); + printf(" scheduling next FB/SB detection task with delay %u\n", delay); + if (abs(last_fb->freq_diff) < fbs.req.freq_err_thresh2 && + last_fb->snr > FB1_SNR_THRESH) { + /* synchronize before reading SB */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + synchronize_tdma(&l1s.serving_cell); + tdma_schedule_set(delay, sb_sched_set, 0); + } else + tdma_schedule_set(delay, fb_sched_set, 1); + } else + l1s_compl_sched(L1_COMPL_FB); + } + + return 0; +} + +/* FB detection */ +static const struct tdma_sched_item fb_sched_set[] = { + SCHED_ITEM_DT(l1s_fbdet_cmd, 0, 0, 0), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 2), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 3), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 4), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 5), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 6), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 7), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 8), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 9), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 10), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 11), SCHED_END_FRAME(), + SCHED_ITEM(l1s_fbdet_resp, -4, 0, 12), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +/* Asynchronous completion handler for FB detection */ +static void l1a_fb_compl(__unused enum l1_compl c) +{ + if (last_fb->attempt >= 13) { + /* FB detection failed, signal this via L1CTL */ + l1ctl_fbsb_resp(255); + return; + } + + /* FIME: use l1s.neigh_cell[fbs.cinfo_idx] */ + fbinfo2cellinfo(&l1s.serving_cell, last_fb); + + /* send FBSB_CONF success message via L1CTL */ + l1ctl_fbsb_resp(0); +} + +void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req) +{ + /* copy + endian convert request data */ + fbs.req.band_arfcn = ntohs(req->band_arfcn); + fbs.req.timeout = ntohs(req->timeout); + fbs.req.freq_err_thresh1 = ntohs(req->freq_err_thresh1); + fbs.req.freq_err_thresh2 = ntohs(req->freq_err_thresh2); + fbs.req.num_freqerr_avg = req->num_freqerr_avg; + fbs.req.flags = req->flags; + fbs.req.sync_info_idx = req->sync_info_idx; + fbs.req.rxlev_exp = req->rxlev_exp; + + /* clear initial frequency error */ + fbs.initial_freq_err = 0; + fbs.fb_retries = 0; + fbs.afc_retries = 0; + + /* Make sure we start at a 'center' AFCDAC output value */ + afc_reset(); + + /* Reset the TOA loop counters */ + toa_reset(); + + if (fbs.req.flags & L1CTL_FBSB_F_FB0) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_FB1) + tdma_schedule_set(base_fn, fb_sched_set, 0); + else if (fbs.req.flags & L1CTL_FBSB_F_SB) + tdma_schedule_set(base_fn, sb_sched_set, 0); + +} + +/* SB for Neighbours in dedicated mode + * + * Only when number of neighbor cells is > 0, perform synchronization. + * + * For each synchronization, l1s.neigh_pm.running is set. In case of an update + * of neighbor cell list, this state is cleared, so a pending sync result would + * be ignored. + * + * After a (new) list of neighbor cells are received, the measurements are not + * yet valid. A valid state flag is used to indicate valid measurements. Until + * there are no valid measurements, the synchronization is not performed. + * + * The task is to scan the 6 strongest neighbor cells by trying to synchronize + * to it. This is done by selecting the strongest unscanned neighbor cell. + * If 6 cells have been scanned or all cells (if less than 6) have been + * scanned, the process clears all 'scanned' flags and starts over with the + * strongest (now the strongest unscanned) cell. + * + * Each synchronization attempt is performed during the "search frame" (IDLE + * frame). The process attempts to sync 11 times to ensure that it hits the + * SCH of the neighbor's BCCH. (The alignment of BCCH shifts after every two + * 26-multiframe in a way that the "search frame" is aligned with the SCH, at + * least once for 11 successive "search frames".) + * + * If the synchronization attempt is successful, the BSIC and neighbor cell + * offset is stored. These are indicated to layer23 with the measurement + * results. + * + * When performing handover to a neighbor cell, the stored offset is used to + * calculate new GSM time and tpu_offset. + */ + +static void select_neigh_cell(void) +{ + uint8_t strongest = 0, strongest_unscanned = 0; + int strongest_i = 0, strongest_unscanned_i = -1; + int num_scanned = 0; + int i; + + /* find strongest cell and strongest unscanned cell and count */ + for (i = 0; i < l1s.neigh_pm.n; i++) { + if (l1s.neigh_pm.level[i] > strongest) { + strongest = l1s.neigh_pm.level[i]; + strongest_i = i; + } + if (!(l1s.neigh_sb.flags_bsic[i] & NEIGH_PM_FLAG_SCANNED)) { + if (l1s.neigh_pm.level[i] > strongest_unscanned) { + strongest_unscanned = l1s.neigh_pm.level[i]; + strongest_unscanned_i = i; + } + } else + num_scanned++; + } + + /* no unscanned cell or we have scanned enough */ + if (strongest_unscanned_i < 0 || num_scanned >= 6) { + /* flag all cells unscanned */ + for (i = 0; i < l1s.neigh_pm.n; i++) + l1s.neigh_sb.flags_bsic[i] &= ~NEIGH_PM_FLAG_SCANNED; + /* use strongest cell to begin scanning with */ + l1s.neigh_sb.index = strongest_i; + } else { + /* use strongest unscanned cell to begin scanning with */ + l1s.neigh_sb.index = strongest_unscanned_i; + } +} + +//MTZ - The function below has been taken from synchronize_tdma in sync.c and modified for whatever seemed necessary +void synchronize_tdma2() +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int sync_test1(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + printf("\n\nMTZ - sync_test1, old_tpu_offset = %d\n\n", l1s.tpu_offset); + + old_tpu_offset = l1s.tpu_offset; + + //l1s.tpu_offset = 3000; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(l1s.tpu_offset); + //tpu_enq_at(0); + //tpu_enq_at(0); + //l1s.tpu_offset = old_tpu_offset; + //l1s.tpu_offset = 2000; + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(l1s.tpu_offset); + + //tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(3000); + //tpu_enq_at(0); + //tpu_enq_at(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(old_tpu_offset); + //tpu_enq_at(0); + + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(2000); + ////tpu_enq_at(0); + ////tpu_enq_at(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(0); + //tpu_enq_at(SWITCH_TIME); + //tpu_enq_sync(old_tpu_offset); + + printf("\n\nMTZ - sync_test1, ending tpu_offset = %d\n\n", l1s.tpu_offset); + + +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int sync_test2(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + printf("\n\nMTZ - sync_test2\n\n"); + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_sync(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + uint32_t tpu_shift; + int ntdma, qbits; + + if ((sb_det == 2)||(sb_det == 4)) { + //printf("\nMTZ - in l1s_neigh_fbsb_sync, old_tpu_offset = %d\n", l1s.tpu_offset); + + /* FIXME: where did this magic 23 come from? */ + nb_fb_toa -= 23; //MTZ - uncomment + + if (nb_fb_toa < 0) { + qbits = (nb_fb_toa + BITS_PER_TDMA) * 4; + ntdma = -1; + } else { + ntdma = (nb_fb_toa) / BITS_PER_TDMA; + qbits = (nb_fb_toa - ntdma * BITS_PER_TDMA) * 4; + } + + tpu_shift = qbits; + + old_tpu_offset = l1s.tpu_offset; + + /* NB detection only works if the TOA of the SB + * is within 0...8. We have to add 75 to get an SB TOA of 4. */ + tpu_shift += 75; //MTZ - uncomment + + tpu_shift = (l1s.tpu_offset + tpu_shift) % QBITS_PER_TDMA; + + //fn_offset = cinfo->fn_offset - 1; + + /* if we're already very close to the end of the TPU frame, the + * next interrupt will basically occur now and we need to + * compensate */ + //if (tpu_shift < SWITCH_TIME) + // fn_offset++; + + //printf("MTZ - old_tpu_offset = %d, tpu_shift = %d, qbits = %d\n", old_tpu_offset, tpu_shift, qbits); + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50) { + //qbits = (nb_fb_toa%BITS_PER_TDMA) * 4; + //tpu_enq_sync((old_tpu_offset + qbits)%5000); + l1s.tpu_offset = tpu_shift; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(tpu_shift); + //tpu_enq_at(0); + } + + if (synchronize_yes) { + l1s.tpu_offset = tpu_shift; + //puts("Synchronize_TDMA\n"); + /* request the TPU to adjust the SYNCHRO and OFFSET registers */ + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + +} +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_cmd(__unused uint8_t p1, __unused uint8_t p2, + uint16_t fb_mode) +{ + + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + //printf("\nMTZ: In neigh_fbsb_cmd, l1s.neigh_pm.n = %d", l1s.neigh_pm.n); + + //---------------------------------------------------------------------------------------- + // + // Important points of note: + // ------------------------- + // + // Temporary variables used for testing: + // + // Right now we have three global variables defined at the beginning of this file + // for testing purposes: det_serving_cell, synchronize_yes and neigh_for_fbsb_det. + // + // det_serving_cell allows for detection of FBSB on serving cell, something which we + // thought should be simpler as the clock is already synched on that cell. + // + // synchronize_yes is a variable with which one can control whether offset and synch + // commands will be sent to the TPU. There was a thought that perhaps the DSP SB + // detection task for dedicated mode might not require synching due to which this + // test variable was created. Also, naturally, detection on the serving cell should + // not require synching but we could be wrong. + // + // neigh_for_fbsb_det is a variable to hardcode the neighbour one wants to detect + // FBSB on. Note that because more functionality is added to the code the mechanism + // for traversing through neighbours using select_neigh_cell and certain flags is + // disturbed for now. Due to this the index is hardcoded in the code below to + // whichever neighbour one wants to detect FBSB on (ordered by signal strength) + // using this variable. + // + // Progress so far: + // + // Right now we are able to detect FB in dedicated mode even for neighbours. One + // additional thing we have done to aid us is that we have added 3 more frames to + // our idle frame by taking up some TCH channels (this can be seen in mframe_sched). + // The call doesn't get dropped if synchronization is not performed except for very + // few cases which we need not worry about for now. However, when synchronization + // is performed the call gets dropped without exception. + // + // Few points where the problem could lie: + // + // 1) Perhaps we are not using the TCH_SB_DSP_TASK correctly. Perhaps some other + // locations in the API also need to be written to when issuing the command in + // addition to dsp_api.db_w->d_task_md. Perhaps we are not reading from the correct + // location when checking the response. + // + // 2) Perhaps we need to start the SB detection task some quarter-bits or timeslots + // before the actual SB appears. + // + // 3) Timing issues. This is the most likely cause as we haven't really been able + // to fully understand and implement timing (if it is required, which it seems like + // it is) in the code. If timing is required then this code is quite superficial. + // In idle mode functions such as afc_correct and procedures requiring frequency + // difference are used to before calling SB detection task which are not done here. + // + // Timing issues seems to be the most likely problem. Anyone wishing to solve the + // SB detection issue should try to focus on this problem, understand how + // synchronization is performed in idle mode and how we can do that in dedicated + // mode and be back within 4 frames as after that the traffic needs to resume. + // + //---------------------------------------------------------------------------------------- + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + index = l1s.neigh_sb.index; + } + + //MTZ - putting this for now as we wanted to repeatedly detect the remaining ones - remove + while (!((l1s.neigh_sb.flags_bsic[index] & NEIGH_PM_FLAG_BSIC) == 0)) { + printf("\nMTZ: BSIC has been decoded for ARFCN %d (flags_bsic[%d] = %d)\n\n", l1s.neigh_pm.band_arfcn[index], index, l1s.neigh_sb.flags_bsic[index]); + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + select_neigh_cell(); + index = l1s.neigh_sb.index; + } + + //MTZ - This index variable is used to hardcode the neighbour cell for now + //index = neigh_for_fbsb_det; + + if (sb_det == 0) { + + //l1s.fb.mode = fb_mode; + +// if (det_serving_cell) +// printf(" - detect FB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); +// else +// printf(" - detect FB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_FB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 5); //MTZ - Original - don't think works + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_FB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_FB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + } else if ((sb_det == 2)||(sb_det == 4)) { + +// if (det_serving_cell) +// printf(" - detect SB arfcn %d (serving cell) (#%d) %d dbm\n", l1s.serving_cell.arfcn, l1s.neigh_sb.count, rxlev2dbm(fbs.req.rxlev_exp)); +// else +// printf(" - detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + //MTZ - This is a variable for the testing phase whether to send sync commands or not + //if (synchronize_yes) + // synchronize_tdma2(); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + if (det_serving_cell) + rffe_compute_gain(rxlev2dbm(fbs.req.rxlev_exp), CAL_DSP_TGT_BB_LVL); + else + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); //MTZ - Commented originally + dsp_api.ndb->d_fb_mode = 0; + +// //MTZ - Experimenting +// dsp_api.ndb->a_sync_demod[D_TOA] = nb_fb_toa; +// dsp_api.ndb->a_sync_demod[D_PM] = nb_fb_pm; +// dsp_api.ndb->a_sync_demod[D_ANGLE] = nb_fb_angle; +// dsp_api.ndb->a_sync_demod[D_SNR] = nb_fb_snr; + + + /* Program TPU */ + //l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); //MTZ - Original - don't think works + if (det_serving_cell) + l1s_rx_win_ctrl(l1s.serving_cell.arfcn, L1_RXWIN_SB26, 0); + else + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 0); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + } + return 0; +} + +/* scheduler callback to issue a FB detection task to the DSP */ +static int l1s_neigh_fbsb_resp(__unused uint8_t p1, uint8_t attempt, + uint16_t fb_mode) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + uint8_t bsic; + int sb_found = 0; + + if (sb_det == 0) { + if (!dsp_api.ndb->d_fb_det) { + printf("MTZ: ARFCN %d (index %d - #%d) FB found = 0\n", l1s.neigh_pm.band_arfcn[index], index, l1s.neigh_sb.count); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + + } else { + nb_fb_toa = dsp_api.ndb->a_sync_demod[D_TOA]; + nb_fb_pm = dsp_api.ndb->a_sync_demod[D_PM]; + nb_fb_angle = dsp_api.ndb->a_sync_demod[D_ANGLE]; + nb_fb_snr = dsp_api.ndb->a_sync_demod[D_SNR]; + printf("MTZ: ARFCN %d (#%d) FB found = 1 >>> nb_fb_toa = %d\n\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, nb_fb_toa); + sb_det = 1; + } + + //l1s_reset_hw(); + tdma_sched_reset(); + } else { + + if ((sb_det == 2)||(sb_det == 4)) { + /* check if sync was successful */ + //if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + // printf("\nSB found1 = 0\n"); + //} else { + // printf("\nSB found1 = 1\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + // printf("\n\n-------------------------------\nSB found1 = 1\n-------------------------------\n\n"); + //} + + if (dsp_api.ndb->a_sch26[0] & (1<<B_SCH_CRC)) { + printf("\nSB found = 0 (ARFCN %d)\n\n", l1s.neigh_pm.band_arfcn[index]); + } else { + sb_found = 1; + sb = dsp_api.ndb->a_sch26[3] | dsp_api.ndb->a_sch26[4] << 16; + bsic = (sb >> 2) & 0x3f; + total_sb_det++; + //printf("=> SB 0x%08"PRIx32": BSIC=%u \n\n", sb, bsic); + printf("\n----------------------------------------------------------------\nSB found = 1 (ARFCN %d) => SB 0x%08"PRIx32": BSIC=%u (Total=%d)\n----------------------------------------------------------------\n\n", l1s.neigh_pm.band_arfcn[index], sb, bsic, total_sb_det); + l1s.neigh_sb.flags_bsic[index] = bsic | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + } + + if ((sb_det == 2)||(sb_det == 4)) { + + //MTZ - testing this - possibly remove + if (nb_fb_toa >= 50); + l1s.tpu_offset = old_tpu_offset; + + if (synchronize_yes) { + l1s.tpu_offset = old_tpu_offset; + tpu_enq_at(SWITCH_TIME); + tpu_enq_sync(l1s.tpu_offset); + } + } + + if ((sb_det == 4)||(sb_found == 1)) { + l1s.neigh_sb.count = 0; + //MTZ - need to change this statement based on detection + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + if (sb_found == 0) + printf("\n\n"); + } + + } + + if ((sb_det == 4)||(sb_found == 1)) + sb_det = 0; + else + sb_det++; + } + + return 0; +} + +static int l1s_neigh_sb_cmd(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint8_t last_gain; + + printf("\n\n\nMTZ: In neigh_sb_cmd, l1s.neigh_pm.n = %d\n\n\n", l1s.neigh_pm.n); + + if (l1s.neigh_pm.n == 0) + return 0; + + /* if measurements are not yet valid, wait */ + if (!l1s.neigh_pm.valid) + return 0; + + /* check for cell to sync to */ + if (l1s.neigh_sb.count == 0) { + /* there is no cell selected, search for cell */ + select_neigh_cell(); + } + + printf("detect SB arfcn %d (#%d) %d dbm\n", l1s.neigh_pm.band_arfcn[index], l1s.neigh_sb.count, rxlev2dbm(l1s.neigh_pm.level[index])); + + last_gain = rffe_get_gain(); + + /* Tell the RF frontend to set the gain appropriately */ + rffe_compute_gain(rxlev2dbm(l1s.neigh_pm.level[index]), CAL_DSP_TGT_BB_LVL); + + /* Program DSP */ + dsp_api.db_w->d_task_md = TCH_SB_DSP_TASK; /* maybe with I/Q swap? */ +// dsp_api.db_w->d_task_md = dsp_task_iq_swap(TCH_SB_DSP_TASK, l1s.neigh_pm.band_arfcn[index], 0); + dsp_api.ndb->d_fb_mode = 0; + + /* Program TPU */ + l1s_rx_win_ctrl(l1s.neigh_pm.band_arfcn[index], L1_RXWIN_SB26, 5); + + /* restore last gain */ + rffe_set_gain(last_gain); + + l1s.neigh_sb.running = 1; + + return 0; +} + +static int l1s_neigh_sb_resp(__unused uint8_t p1, __unused uint8_t p2, + __unused uint16_t p3) +{ + int index = l1s.neigh_sb.index; + uint32_t sb; + + if (l1s.neigh_pm.n == 0 || !l1s.neigh_sb.running) + goto out; + + /* check if sync was successful */ + if (dsp_api.db_r->a_sch[0] & (1<<B_SCH_CRC)) { + printf("SB error arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* next sync */ + if (++l1s.neigh_sb.count == 11) { + l1s.neigh_sb.count = 0; + l1s.neigh_sb.flags_bsic[index] |= NEIGH_PM_FLAG_SCANNED; + } + } else { + l1s.neigh_sb.count = 0; + + read_sb_result(last_fb, 1); + sb = dsp_api.db_r->a_sch[3] | dsp_api.db_r->a_sch[4] << 16; + l1s.neigh_sb.flags_bsic[index] = + l1s_decode_sb(&fbs.mon.time, sb) + | NEIGH_PM_FLAG_BSIC | NEIGH_PM_FLAG_SCANNED; + printf("SB OK!!!!!! arfcn %d\n", l1s.neigh_pm.band_arfcn[index]); + + /* store time offset */ + } + +out: + l1s.neigh_sb.running = 0; + + dsp_api.r_page_used = 1; + + return 0; + +} + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(l1s_neigh_sb_cmd, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_FRAME(), +// SCHED_ITEM(l1s_neigh_sb_resp, -4, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + +/* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +const struct tdma_sched_item neigh_sync_sched_set[] = { + SCHED_ITEM_DT(l1s_neigh_fbsb_sync, 1, 0, 1), SCHED_END_FRAME(), + SCHED_ITEM_DT(l1s_neigh_fbsb_cmd, 1, 0, 1), SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_END_FRAME(), + SCHED_ITEM(l1s_neigh_fbsb_resp, -4, 0, 1), SCHED_END_FRAME(), + SCHED_END_SET() +}; + +///* NOTE: Prio 1 is below TCH's RX+TX prio 0 */ +//const struct tdma_sched_item neigh_sync_sched_set[] = { +// SCHED_ITEM_DT(sync_test1, 1, 0, 1), SCHED_END_FRAME(), +//// SCHED_END_FRAME(), +//// SCHED_END_FRAME(), +//// SCHED_ITEM_DT(sync_test2, 1, 0, 1), SCHED_END_FRAME(), +// SCHED_END_SET() +//}; + + +static __attribute__ ((constructor)) void l1s_prim_fbsb_init(void) +{ + l1s.completion[L1_COMPL_FB] = &l1a_fb_compl; +} |