git.gag.com Git - debian/gnuradio/blob - usrp2/firmware/apps/app_common_v2.c

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2007,2008 Free Software Foundation, Inc.
   4  *
   5  * This program is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, either version 3 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #ifdef HAVE_CONFIG_H
  20 #include "config.h"
  21 #endif
  22
  23 #include "app_common_v2.h"
  24 #include "buffer_pool.h"
  25 #include "memcpy_wa.h"
  26 #include "ethernet.h"
  27 #include "nonstdio.h"
  28 #include "print_rmon_regs.h"
  29 #include "db.h"
  30 #include "db_base.h"
  31 #include "clocks.h"
  32 #include "u2_init.h"
  33 #include <string.h>
  34
  35 volatile bool link_is_up = false;       // eth handler sets this
  36 int cpu_tx_buf_dest_port = PORT_ETH;
  37
  38 // If this is non-zero, this dbsm could be writing to the ethernet
  39 dbsm_t *ac_could_be_sending_to_eth;
  40
  41 static unsigned char exp_seqno __attribute__((unused)) = 0;
  42
  43 static bool
  44 burn_mac_addr(const op_burn_mac_addr_t *p)
  45 {
  46   return ethernet_set_mac_addr(&p->addr);
  47 }
  48
  49 static bool
  50 config_mimo_cmd(const op_config_mimo_t *p)
  51 {
  52   clocks_mimo_config(p->flags);
  53   return true;
  54 }
  55
  56 void
  57 set_reply_hdr(u2_eth_packet_t *reply_pkt, u2_eth_packet_t const *cmd_pkt)
  58 {
  59   reply_pkt->ehdr.dst = cmd_pkt->ehdr.src;
  60   reply_pkt->ehdr.ethertype = U2_ETHERTYPE;
  61   reply_pkt->thdr.flags = 0;
  62   reply_pkt->thdr.fifo_status = 0;      // written by protocol engine
  63   reply_pkt->thdr.seqno = 0;            // written by protocol engine
  64   reply_pkt->thdr.ack = 0;              // written by protocol engine
  65   u2p_set_word0(&reply_pkt->fixed, 0, CONTROL_CHAN);
  66   reply_pkt->fixed.timestamp = timer_regs->time;
  67 }
  68
  69 static void
  70 send_reply(unsigned char *reply, size_t reply_len)
  71 {
  72   if (reply_len < 64)
  73     reply_len = 64;
  74
  75   // wait for buffer to become idle
  76   hal_set_leds(0x4, 0x4);
  77   while((buffer_pool_status->status & BPS_IDLE(CPU_TX_BUF)) == 0)
  78     ;
  79   hal_set_leds(0x0, 0x4);
  80
  81   // copy reply into CPU_TX_BUF
  82   memcpy_wa(buffer_ram(CPU_TX_BUF), reply, reply_len);
  83
  84   // wait until nobody else is sending to the ethernet
  85   if (ac_could_be_sending_to_eth){
  86     hal_set_leds(0x8, 0x8);
  87     dbsm_wait_for_opening(ac_could_be_sending_to_eth);
  88     hal_set_leds(0x0, 0x8);
  89   }
  90
  91   if (0){
  92     printf("sending_reply to port %d, len = %d\n", cpu_tx_buf_dest_port, reply_len);
  93     print_buffer(buffer_ram(CPU_TX_BUF), reply_len/4);
  94   }
  95
  96   // fire it off
  97   bp_send_from_buf(CPU_TX_BUF, cpu_tx_buf_dest_port, 1, 0, reply_len/4);
  98
  99   // wait for it to complete (not long, it's a small pkt)
 100   while((buffer_pool_status->status & (BPS_DONE(CPU_TX_BUF) | BPS_ERROR(CPU_TX_BUF))) == 0)
 101     ;
 102
 103   bp_clear_buf(CPU_TX_BUF);
 104 }
 105
 106
 107 static size_t
 108 op_id_cmd(const op_generic_t *p,
 109           void *reply_payload, size_t reply_payload_space)
 110 {
 111   op_id_reply_t *r = (op_id_reply_t *) reply_payload;
 112   if (reply_payload_space < sizeof(*r)) // no room
 113     return 0;
 114
 115   // Build reply subpacket
 116
 117   r->opcode = OP_ID_REPLY;
 118   r->len = sizeof(op_id_reply_t);
 119   r->rid = p->rid;
 120   r->addr = *ethernet_mac_addr();
 121   r->hw_rev = (u2_hw_rev_major << 8) | u2_hw_rev_minor;
 122   // r->fpga_md5sum = ; // FIXME
 123   // r->sw_md5sum = ;   // FIXME
 124
 125   return r->len;
 126 }
 127
 128
 129 static size_t
 130 config_tx_v2_cmd(const op_config_tx_v2_t *p,
 131                  void *reply_payload, size_t reply_payload_space)
 132 {
 133   op_config_tx_reply_v2_t *r = (op_config_tx_reply_v2_t *) reply_payload;
 134   if (reply_payload_space < sizeof(*r))
 135     return 0;                                   // no room
 136
 137   struct tune_result    tune_result;
 138   memset(&tune_result, 0, sizeof(tune_result));
 139
 140   bool ok = true;
 141
 142   if (p->valid & CFGV_GAIN){
 143     ok &= db_set_gain(tx_dboard, p->gain);
 144   }
 145
 146   if (p->valid & CFGV_FREQ){
 147     bool was_streaming = is_streaming();
 148     if (was_streaming)
 149       stop_rx_cmd();
 150
 151     u2_fxpt_freq_t f = u2_fxpt_freq_from_hilo(p->freq_hi, p->freq_lo);
 152     bool tune_ok = db_tune(tx_dboard, f, &tune_result);
 153     ok &= tune_ok;
 154     print_tune_result("Tx", tune_ok, f, &tune_result);
 155
 156     if (was_streaming)
 157       restart_streaming();
 158   }
 159
 160   if (p->valid & CFGV_INTERP_DECIM){
 161     int interp = p->interp;
 162     int hb1 = 0;
 163     int hb2 = 0;
 164
 165     if (!(interp & 1)){
 166       hb2 = 1;
 167       interp = interp >> 1;
 168     }
 169
 170     if (!(interp & 1)){
 171       hb1 = 1;
 172       interp = interp >> 1;
 173     }
 174
 175     if (p->interp < MIN_INTERP || p->interp > MAX_INTERP)
 176       ok = false;
 177     else {
 178       dsp_tx_regs->interp_rate = (hb1<<9) | (hb2<<8) | interp;
 179       // printf("Interp: %d, register %d\n", p->interp, (hb1<<9) | (hb2<<8) | interp);
 180     }
 181   }
 182
 183   if (p->valid & CFGV_SCALE_IQ){
 184     dsp_tx_regs->scale_iq = p->scale_iq;
 185   }
 186
 187   // Build reply subpacket
 188
 189   r->opcode = OP_CONFIG_TX_REPLY_V2;
 190   r->len = sizeof(*r);
 191   r->rid = p->rid;
 192   r->ok = ok;
 193   r->inverted = tune_result.inverted;
 194   r->baseband_freq_hi = u2_fxpt_freq_hi(tune_result.baseband_freq);
 195   r->baseband_freq_lo = u2_fxpt_freq_lo(tune_result.baseband_freq);
 196   r->duc_freq_hi = u2_fxpt_freq_hi(tune_result.dxc_freq);
 197   r->duc_freq_lo = u2_fxpt_freq_lo(tune_result.dxc_freq);
 198   r->residual_freq_hi = u2_fxpt_freq_hi(tune_result.residual_freq);
 199   r->residual_freq_lo = u2_fxpt_freq_lo(tune_result.residual_freq);
 200   return r->len;
 201 }
 202
 203 static size_t
 204 config_rx_v2_cmd(const op_config_rx_v2_t *p,
 205                  void *reply_payload, size_t reply_payload_space)
 206 {
 207   op_config_rx_reply_v2_t *r = (op_config_rx_reply_v2_t *) reply_payload;
 208   if (reply_payload_space < sizeof(*r))
 209     return 0;                           // no room
 210
 211   struct tune_result    tune_result;
 212   memset(&tune_result, 0, sizeof(tune_result));
 213
 214   bool ok = true;
 215
 216   if (p->valid & CFGV_GAIN){
 217     ok &= db_set_gain(rx_dboard, p->gain);
 218   }
 219
 220   if (p->valid & CFGV_FREQ){
 221     bool was_streaming = is_streaming();
 222     if (was_streaming)
 223       stop_rx_cmd();
 224
 225     u2_fxpt_freq_t f = u2_fxpt_freq_from_hilo(p->freq_hi, p->freq_lo);
 226     bool tune_ok = db_tune(rx_dboard, f, &tune_result);
 227     ok &= tune_ok;
 228     print_tune_result("Rx", tune_ok, f, &tune_result);
 229
 230     if (was_streaming)
 231       restart_streaming();
 232   }
 233
 234   if (p->valid & CFGV_INTERP_DECIM){
 235     int decim = p->decim;
 236     int hb1 = 0;
 237     int hb2 = 0;
 238
 239     if(!(decim & 1)) {
 240       hb2 = 1;
 241       decim = decim >> 1;
 242     }
 243
 244     if(!(decim & 1)) {
 245       hb1 = 1;
 246       decim = decim >> 1;
 247     }
 248
 249     if (decim < MIN_DECIM || decim > MAX_DECIM)
 250       ok = false;
 251     else {
 252       dsp_rx_regs->decim_rate = (hb1<<9) | (hb2<<8) | decim;
 253       // printf("Decim: %d, register %d\n", p->decim, (hb1<<9) | (hb2<<8) | decim);
 254     }
 255   }
 256
 257   if (p->valid & CFGV_SCALE_IQ){
 258     dsp_rx_regs->scale_iq = p->scale_iq;
 259   }
 260
 261   // Build reply subpacket
 262
 263   r->opcode = OP_CONFIG_RX_REPLY_V2;
 264   r->len = sizeof(*r);
 265   r->rid = p->rid;
 266   r->ok = ok;
 267   r->inverted = tune_result.inverted;
 268   r->baseband_freq_hi = u2_fxpt_freq_hi(tune_result.baseband_freq);
 269   r->baseband_freq_lo = u2_fxpt_freq_lo(tune_result.baseband_freq);
 270   r->ddc_freq_hi = u2_fxpt_freq_hi(tune_result.dxc_freq);
 271   r->ddc_freq_lo = u2_fxpt_freq_lo(tune_result.dxc_freq);
 272   r->residual_freq_hi = u2_fxpt_freq_hi(tune_result.residual_freq);
 273   r->residual_freq_lo = u2_fxpt_freq_lo(tune_result.residual_freq);
 274
 275   return r->len;
 276 }
 277
 278 static size_t
 279 read_time_cmd(const op_generic_t *p,
 280               void *reply_payload, size_t reply_payload_space)
 281 {
 282   op_read_time_reply_t *r = (op_read_time_reply_t *) reply_payload;
 283   if (reply_payload_space < sizeof(*r))
 284     return 0;                                   // no room
 285
 286   r->opcode = OP_READ_TIME_REPLY;
 287   r->len = sizeof(*r);
 288   r->rid = p->rid;
 289   r->time = timer_regs->time;
 290
 291   return r->len;
 292 }
 293
 294 static void
 295 fill_db_info(u2_db_info_t *p, const struct db_base *db)
 296 {
 297   p->dbid = db->dbid;
 298   p->freq_min_hi = u2_fxpt_freq_hi(db->freq_min);
 299   p->freq_min_lo = u2_fxpt_freq_lo(db->freq_min);
 300   p->freq_max_hi = u2_fxpt_freq_hi(db->freq_max);
 301   p->freq_max_lo = u2_fxpt_freq_lo(db->freq_max);
 302   p->gain_min = db->gain_min;
 303   p->gain_max = db->gain_max;
 304   p->gain_step_size = db->gain_step_size;
 305 }
 306
 307 static size_t
 308 dboard_info_cmd(const op_generic_t *p,
 309                 void *reply_payload, size_t reply_payload_space)
 310 {
 311   op_dboard_info_reply_t *r = (op_dboard_info_reply_t *) reply_payload;
 312   if (reply_payload_space < sizeof(*r))
 313     return 0;                                   // no room
 314
 315   r->opcode = OP_DBOARD_INFO_REPLY;
 316   r->len = sizeof(*r);
 317   r->rid = p->rid;
 318   r->ok = true;
 319
 320   fill_db_info(&r->tx_db_info, tx_dboard);
 321   fill_db_info(&r->rx_db_info, rx_dboard);
 322
 323   return r->len;
 324 }
 325
 326 static size_t
 327 generic_reply(const op_generic_t *p,
 328               void *reply_payload, size_t reply_payload_space,
 329               bool ok)
 330 {
 331   op_generic_t *r = (op_generic_t *) reply_payload;
 332   if (reply_payload_space < sizeof(*r))
 333     return 0;                                   // no room
 334
 335   r->opcode = p->opcode | OP_REPLY_BIT;
 336   r->len = sizeof(*r);
 337   r->rid = p->rid;
 338   r->ok = ok;
 339
 340   return r->len;
 341 }
 342
 343 static size_t
 344 add_eop(void *reply_payload, size_t reply_payload_space)
 345 {
 346   op_generic_t *r = (op_generic_t *) reply_payload;
 347   if (reply_payload_space < sizeof(*r))
 348     return 0;                                   // no room
 349
 350   r->opcode = OP_EOP;
 351   r->len = sizeof(*r);
 352   r->rid = 0;
 353   r->ok =  0;
 354
 355   return r->len;
 356 }
 357
 358 void
 359 handle_control_chan_frame(u2_eth_packet_t *pkt, size_t len)
 360 {
 361   unsigned char reply[sizeof(u2_eth_packet_t) + 4 * sizeof(u2_subpkt_t)] _AL4;
 362   unsigned char *reply_payload = &reply[sizeof(u2_eth_packet_t)];
 363   int reply_payload_space = sizeof(reply) - sizeof(u2_eth_packet_t);
 364
 365   // initialize reply
 366   memset(reply, 0, sizeof(reply));
 367   set_reply_hdr((u2_eth_packet_t *) reply, pkt);
 368
 369   // point to beginning of payload (subpackets)
 370   unsigned char *payload = ((unsigned char *) pkt) + sizeof(u2_eth_packet_t);
 371   int payload_len = len - sizeof(u2_eth_packet_t);
 372
 373   size_t subpktlen = 0;
 374
 375   while (payload_len >= sizeof(op_generic_t)){
 376     const op_generic_t *gp = (const op_generic_t *) payload;
 377     subpktlen = 0;
 378
 379     switch(gp->opcode){
 380     case OP_EOP:                // end of subpackets
 381       goto end_of_subpackets;
 382
 383     case OP_ID:
 384       subpktlen = op_id_cmd(gp, reply_payload, reply_payload_space);
 385       break;
 386
 387     case OP_CONFIG_TX_V2:
 388       subpktlen = config_tx_v2_cmd((op_config_tx_v2_t *) payload,
 389                                    reply_payload, reply_payload_space);
 390       break;
 391
 392     case OP_CONFIG_RX_V2:
 393       subpktlen = config_rx_v2_cmd((op_config_rx_v2_t *) payload,
 394                                    reply_payload, reply_payload_space);
 395       break;
 396
 397     case OP_START_RX_STREAMING:
 398       start_rx_streaming_cmd(&pkt->ehdr.src, (op_start_rx_streaming_t *) payload);
 399       subpktlen = generic_reply(gp, reply_payload, reply_payload_space, true);
 400       break;
 401
 402     case OP_STOP_RX:
 403       stop_rx_cmd();
 404       subpktlen = generic_reply(gp, reply_payload, reply_payload_space, true);
 405       break;
 406
 407     case OP_BURN_MAC_ADDR:
 408       subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
 409                                 burn_mac_addr((op_burn_mac_addr_t *) payload));
 410       break;
 411
 412     case OP_CONFIG_MIMO:
 413       subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
 414                                 config_mimo_cmd((op_config_mimo_t *) payload));
 415       break;
 416
 417     case OP_READ_TIME:
 418       subpktlen = read_time_cmd(gp, reply_payload, reply_payload_space);
 419       break;
 420
 421     case OP_DBOARD_INFO:
 422       subpktlen = dboard_info_cmd(gp, reply_payload, reply_payload_space);
 423       break;
 424
 425     default:
 426       printf("app_common_v2: unhandled opcode = %d\n", gp->opcode);
 427       break;
 428     }
 429
 430     int t = (gp->len + 3) & ~3;         // bump to a multiple of 4
 431     payload += t;
 432     payload_len -= t;
 433
 434     subpktlen = (subpktlen + 3) & ~3;   // bump to a multiple of 4
 435     reply_payload += subpktlen;
 436     reply_payload_space -= subpktlen;
 437   }
 438
 439  end_of_subpackets:
 440
 441   // add the EOP marker
 442   subpktlen = add_eop(reply_payload, reply_payload_space);
 443   subpktlen = (subpktlen + 3) & ~3;     // bump to a multiple of 4
 444   reply_payload += subpktlen;
 445   reply_payload_space -= subpktlen;
 446
 447   send_reply(reply, reply_payload - reply);
 448 }
 449
 450
 451 /*
 452  * Called when an ethernet packet is received.
 453  * Return true if we handled it here, otherwise
 454  * it'll be passed on to the DSP Tx pipe
 455  */
 456 bool
 457 eth_pkt_inspector(dbsm_t *sm, int bufno)
 458 {
 459   u2_eth_packet_t *pkt = (u2_eth_packet_t *) buffer_ram(bufno);
 460   size_t byte_len = (buffer_pool_status->last_line[bufno] - 3) * 4;
 461
 462   //static size_t last_len = 0;
 463
 464   // hal_toggle_leds(0x1);
 465
 466   // inspect rcvd frame and figure out what do do.
 467
 468   if (pkt->ehdr.ethertype != U2_ETHERTYPE)
 469     return true;        // ignore, probably bogus PAUSE frame from MAC
 470
 471   int chan = u2p_chan(&pkt->fixed);
 472
 473   switch (chan){
 474   case CONTROL_CHAN:
 475     handle_control_chan_frame(pkt, byte_len);
 476     return true;        // we handled the packet
 477     break;
 478
 479   case 0:
 480   default:
 481 #if 0
 482     if (last_len != 0){
 483       if (byte_len != last_len){
 484         printf("Len: %d last: %d\n", byte_len, last_len);
 485       }
 486     }
 487     last_len = byte_len;
 488
 489     if((pkt->thdr.seqno) == exp_seqno){
 490       exp_seqno++;
 491       //putchar('.');
 492     }
 493     else {
 494       // putchar('S');
 495       //printf("S%d %d ",exp_seqno,pkt->thdr.seqno);
 496       exp_seqno = pkt->thdr.seqno + 1;
 497     }
 498 #endif
 499     return false;       // pass it on to Tx DSP
 500     break;
 501   }
 502 }
 503
 504 /*
 505  * Called when eth phy state changes (w/ interrupts disabled)
 506  */
 507 void
 508 link_changed_callback(int speed)
 509 {
 510   link_is_up = speed != 0;
 511   hal_set_leds(link_is_up ? 0x20 : 0x0, 0x20);
 512   printf("\neth link changed: speed = %d\n", speed);
 513 }
 514
 515
 516 void
 517 print_tune_result(char *msg, bool tune_ok,
 518                   u2_fxpt_freq_t target_freq, struct tune_result *r)
 519 {
 520 #if 0
 521   printf("db_tune %s %s\n", msg, tune_ok ? "true" : "false");
 522   putstr("  target_freq   "); print_fxpt_freq(target_freq); newline();
 523   putstr("  baseband_freq "); print_fxpt_freq(r->baseband_freq); newline();
 524   putstr("  dxc_freq      "); print_fxpt_freq(r->dxc_freq); newline();
 525   putstr("  residual_freq "); print_fxpt_freq(r->residual_freq); newline();
 526   printf("  inverted      %s\n", r->inverted ? "true" : "false");
 527 #endif
 528 }