stfa_ics.c

来自「软件无线电的平台」· C语言 代码 · 共 919 行 · 第 1/3 页

	TX2RX( 3 * private->slot_bytes / 2 - private->dma_bytes );
      // Copy the beginning of the buffer to the end if the slot starts in the 
      // last 3/2 slot from the end of the DMA
      if ( b > 0 ){
	memcpy( DMA_END_RX, DMA_BEGIN_RX, b );
      }
    }
  }

  // Process an evenutal tx-port
  PR_DBG( 4, "Send: %i, Rcv: %i\n", slot_send, slot_rcv );
  if ( private->notice_sdb[ slot_send ] >= 0 ) {
    int err_prep;
    PR_DBG( 4, "Sending slot-%i-notice to %i\n",
            slot_send, private->notice_sdb[ slot_send ] );
    err_prep = swr_sdb_send_msg( private->notice_sdb[ slot_send ], 
				 SUBS_MSG_PREPARE, NULL, -1 );
    if ( !err_prep ){
      swr_sdb_send_msg( private->notice_sdb[ slot_send ],
			SUBS_MSG_DATA, (void*)-1, context->id );
    } else {
      PR_DBG( 2, "Couldn't prepare for reason: %i\n",
	      err_prep );
    }
  }
  if ( port_in( slot_send ).sdb_id == -1 ){
    // Switch the slot to receiving
    PR_DBG( 4, "Putting slot %i to reception\n", slot_send );
    // Put the memset in again, as TX_OFF is now zero, and byte-wise
    // setting to zero is OK
    memset( private->buffer_tx + BUFFER_OFFSET_TX( slot_send ),
     	    TX_OFF, private->slot_bytes );
  }
  
  // Process the notice function
  if ( private->notice_f[ slot_send ] ) {
    PR_DBG( 4, "Calling notice-function on slot %i\n", slot_send );
    private->notice_f[ slot_send ]( SUBS_MSG_USER, NULL, context->id );
  }

  // Process an eventual rx-port
  if ( port_out( slot_rcv ).sdb_id >= 0 ) {
    int err_prep;
    PR_DBG( 4, "Sending rcvd slot %i\n", slot_rcv );
    err_prep = swr_sdb_send_msg( port_out( slot_rcv ).sdb_id, 
				 SUBS_MSG_PREPARE, NULL, -1 );
    if ( !err_prep ){
      PR_DBG( 4, "Chain is prepared, sending\n" );
      port_out( slot_rcv ).flags |= SWR_PORT_DATA;
    } else {
      PR_DBG( 4, "Couldn't prepare for reason %i\n",
	      err_prep );
    }
  }

  // Count how many frames went by
  if ( !slot_send ) {
    swr_sdb_get_stats_struct( context->id, (void**)&stats );
    stats->frame_count++;
    swr_sdb_free_stats_struct( context->id, (void**)&stats );
  }
}



/**
 * When a slot is ready to send, the subsystem calls this function.
 */
int spc_send( swr_sdb_t *context ) {
  int p, nbr_ports, b;
  stats_t *stats;

  swr_sdb_get_stats_struct( context->id, (void**)&stats );
  nbr_ports = stats->slots_per_frame;
  swr_sdb_free_stats_struct( context->id, (void**)&stats );
  // The only thing we need to think about is a wrap-around of the
  // buffer...
  // The buffer is: SLOT_BYTES + DMA_BYTES + SLOT_BYTES
  // where DMA_BYTES = SLOT_BYTES * SLOTS_PER_FRAME
  // and the SLOT_BYTES before and after serve as a simplification
  // of the ring-buffer.
  for ( p=0; p < nbr_ports; p++ ) {
    if ( port_in(p).flags & SWR_PORT_DATA ) {
      PR_DBG( 4, "stfa[%i], BO[%i]: %i\n", private->nbr_stfa, 
	      p, BUFFER_OFFSET_TX( p ) );
      PR_DBG( 4, "%i %i %i %i %i\n",
	      private->offset_rcv_blocks, private->rx_tx_delay_blocks,
	      private->rx_tx_delay_bytes, private->offset_rcv_bytes,
	      private->offset_send_bytes );
      // Make sure the wrap-around is correct:
      // For the beginning of the buffer
      b = DMA_OFFSET_TX( p );
      if ( b < 0 ){
	// The slot starts BEFORE the DMA-region, so copy it to the end
	memcpy( DMA_END_TX + b, DMA_BEGIN_TX + b, -b );
      } else {
	b += private->slot_bytes - private->dma_bytes;
	if ( b > 0 ){
	  // The slot reaches AFTER the DMA-region, so copy it to the beginning
	  memcpy( DMA_BEGIN_TX,
		  DMA_END_TX,
		  b );
	}
      }
      port_in(p).flags &= ~SWR_PORT_DATA;
    }
  }
  return 0;
}



/**
 * Inserts function in the table of notice queues, so that things
 * can roll
 */
void add_notice_func( swr_sdb_t *context, func *f, int port ) {

  if ( ( port >= private->slots_per_frame ) ||
       ( port < 0 ) ) {
    PR_DBG( 1, "Couldn't attach to port %i, out of range\n", port );
    return;
  }
  private->notice_f[ port ] = f;
}



/**
 * Inserts the sdb-id in the table of notice queues, so that things
 * can roll
 */
void add_notice_sdb( swr_sdb_t *context, swr_sdb_id ret, int port ) {

  if ( ( port >= private->slots_per_frame ) ||
       ( port < 0 ) ) {
    PR_DBG( 1, "Couldn't attach to port %i, out of range\n", port );
    return;
  }
  private->notice_sdb[ port ] = ret;
}


/**
 * Dispatch Go, Stop
 */
int spc_custom_msg( swr_sdb_t *context, swr_usr_msg_t *data, swr_sdb_id ret ) {

  if ( !data ) {
    PR_DBG( 0, "Hmm, empty user-message is not cool for stfa...\n" );
  } else if ( !strcmp( data->id, "ANT" ) ) {
    // Message of the antenna-dispatcher that a slot is ready
    if ( !private->running ) {
      // If we have more than one stfa, a call to swr_stfa_go (sfta_0)
      // will start all antennas. So if we're stopped and not the first
      // stfa, upon reception of a "ANT" message, we go into running mode.
      if ( private->nbr_stfa > 0 ){
        PR_DBG( 1, "Parallel starting of STFA %i\n",
			private->nbr_stfa );
	private->running = 1;
      } else {
        PR_DBG( 0, "Got called while stopped!\n" );
      }
    }
    if ( private->running ) {
      PR_DBG( 4, "Got some dma-buffer, offset = %i\n", private->offset_rcv_blocks );
      spc_slot( context, data->data );
    }
    swr_free( data->data );
  } else if ( !strcmp( data->id, "Go" ) ) {
    // Start the whole thing
    PR_DBG( 2, "Starting the stfa\n" );
    private->running = 1;
    if ( private->stfa_id >= 0 ){
      // If there is another STFA, chain it
      PR_DBG( 1, "Chaining the STFA\n" );
      swr_stfa_go( private->stfa_id );
    } else {
      // We start the antenna which starts all channels
      PR_DBG( 1, "We're the last, starting the channels: %i\n",
	      private->len_frame_blocks );
      swr_ant_start( private->len_frame_blocks );
    }
  //  PR_DBG( 4, "Finished starting the antenna\n" );
  } else if ( !strcmp( data->id, "Stop" ) ) {
    // Stop the whole thing
    PR_DBG( 2, "Stopping the stfa\n" );
    swr_ant_stop( );
    private->running = 0;
  } else if ( !strncmp( data->id, "Notice_func", 11 ) ) {
    // Add a function
    PR_DBG( 2, "Adding a notice-function for port %i\n",
            (int)data->id[11] );
    add_notice_func( context, data->data, (int)data->id[11] );
    data->data = NULL;
  } else if ( !strncmp( data->id, "Notice_sdb", 10 ) ) {
    // Add a queue
    PR_DBG( 2, "Adding a notice to sdb-id %i, port %i\n",
            ret, (int)data->id[10] );
    add_notice_sdb( context, ret, (int)data->id[10] );
  }
  return 0;
}


int spc_finalize( swr_sdb_t *context ) {
  PR_DBG( 2, "Finalizing stfa %i\n", private->nbr_stfa );
  if ( private->running ){
    if ( private->nbr_stfa == 0 ){
      PR_DBG( 0, "Stopping the stfa in extremis!\n" );
    }
    swr_ant_stop( );
  }

  swr_ant_free( private->nbr_stfa );
  if ( !( private->nbr_stfa % 2 ) ){
    PR_DBG( 2, "Freeing RX\n" );
    swr_free( private->buffer_rx );
  }
  PR_DBG( 2, "Freeing TX\n" );
  swr_free( private->buffer_tx );
  PR_DBG( 2, "Freeing finished\n" );
  swr_free( private );
  nbr_stfa--;
  MOD_DEC_USE_COUNT;
  return 0;
}


swr_spc_id_t spc_id;

/**
 * This function is called upon "insmod" and is used to register the
 * different parts of the module to the SPM.
 */
int spc_module_init(void) {
  swr_spc_desc_t *desc;
  int i;

  // Get a description-part from SPM
  // Nbr_input_ports, Nbr_output_ports, Nbr_config_values, Nbr_stats_values
  desc = swr_spc_get_new_desc( MAX_SLOTS, MAX_SLOTS, 15, 7 );
  if ( !desc ) {
    PR_DBG( 0, "Can't initialise the module. This is BAD!\n" );
    return -1;
  }

  UM_CONFIG_INT( "slot_send_offset" );
  UM_CONFIG_INT( "offset_chips_rcv" );
  UM_CONFIG_INT( "offset_samples_rcv" );
  UM_CONFIG_INT( "offset_samples_send" );
  UM_CONFIG_INT( "blocks_per_slot" );
  UM_CONFIG_INT( "slots_per_frame" );
  UM_CONFIG_INT( "guard_period_chips" );
  UM_CONFIG_INT( "attn_tx" );
  UM_CONFIG_INT( "attn_rx" );
  UM_CONFIG_INT( "stfa_id" );
  UM_CONFIG_POINTER( "buffer_rx" );
  UM_CONFIG_INT( "tx" );
  UM_CONFIG_INT( "freq" );
  UM_CONFIG_INT( "freq_diff" );
  UM_CONFIG_INT( "side" );

  UM_STATS_INT( "frame_count" );
  UM_STATS_INT( "blocks_per_slot" );
  UM_STATS_INT( "slots_per_frame" );
  UM_STATS_BLOCK( "buffer_rx" );
  UM_STATS_BLOCK( "buffer_tx" );
  UM_STATS_INT( "offset_chips" );
  UM_STATS_INT( "offset_samples" );

  for ( i=0; i<MAX_SLOTS; i++ ) {
    UM_INPUT( SIG_SYMBOL_COMPLEX, 0 );
    UM_OUTPUT( SIG_SYMBOL_COMPLEX_S32, 0 );
  }

  // Initialise the callback-functions. NULL for not-used functions
  desc->fn_init = spc_init;
  desc->fn_reconfigure = spc_reconfig;
  desc->fn_custom_msg = spc_custom_msg;
  desc->fn_finalize = spc_finalize;
  desc->fn_process_data = spc_send;
  nbr_stfa = 0;

  // And register the module in the SPM
  spc_id = swr_cdb_register_spc( &desc, "stfa_ics" );
  if ( spc_id == SWR_SPM_INVALID_ID ) {
    swr_spc_free_desc( desc );
    PR_DBG( 0, "Couldn't register the module!\n" );
    return 1;
  }
  return 0;
}

void spc_module_exit( void ) {
  PR_DBG( 2, "STFA is going\n" );
  if ( swr_cdb_unregister_spc( spc_id ) < 0 ) {
    PR_DBG( 0, "Still in use somewhere\n" );
  }
}


module_init( spc_module_init );
module_exit( spc_module_exit );