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 sync_to_pps(const op_generic_t *p)
  51 {
  52   timesync_regs->sync_on_next_pps = 1;
  53   putstr("SYNC to PPS\n");
  54   return true;
  55 }
  56
  57 static bool
  58 config_mimo_cmd(const op_config_mimo_t *p)
  59 {
  60   clocks_mimo_config(p->flags);
  61   return true;
  62 }
  63
  64 void
  65 set_reply_hdr(u2_eth_packet_t *reply_pkt, u2_eth_packet_t const *cmd_pkt)
  66 {
  67   reply_pkt->ehdr.dst = cmd_pkt->ehdr.src;
  68   reply_pkt->ehdr.ethertype = U2_ETHERTYPE;
  69   reply_pkt->thdr.flags = 0;
  70   reply_pkt->thdr.fifo_status = 0;      // written by protocol engine
  71   reply_pkt->thdr.seqno = 0;            // written by protocol engine
  72   reply_pkt->thdr.ack = 0;              // written by protocol engine
  73   u2p_set_word0(&reply_pkt->fixed, 0, CONTROL_CHAN);
  74   reply_pkt->fixed.timestamp = timer_regs->time;
  75 }
  76
  77 static void
  78 send_reply(unsigned char *reply, size_t reply_len)
  79 {
  80   if (reply_len < 64)
  81     reply_len = 64;
  82
  83   // wait for buffer to become idle
  84   hal_set_leds(0x4, 0x4);
  85   while((buffer_pool_status->status & BPS_IDLE(CPU_TX_BUF)) == 0)
  86     ;
  87   hal_set_leds(0x0, 0x4);
  88
  89   // copy reply into CPU_TX_BUF
  90   memcpy_wa(buffer_ram(CPU_TX_BUF), reply, reply_len);
  91
  92   // wait until nobody else is sending to the ethernet
  93   if (ac_could_be_sending_to_eth){
  94     hal_set_leds(0x8, 0x8);
  95     dbsm_wait_for_opening(ac_could_be_sending_to_eth);
  96     hal_set_leds(0x0, 0x8);
  97   }
  98
  99   if (0){
 100     printf("sending_reply to port %d, len = %d\n", cpu_tx_buf_dest_port, (int)reply_len);
 101     print_buffer(buffer_ram(CPU_TX_BUF), reply_len/4);
 102   }
 103
 104   // fire it off
 105   bp_send_from_buf(CPU_TX_BUF, cpu_tx_buf_dest_port, 1, 0, reply_len/4);
 106
 107   // wait for it to complete (not long, it's a small pkt)
 108   while((buffer_pool_status->status & (BPS_DONE(CPU_TX_BUF) | BPS_ERROR(CPU_TX_BUF))) == 0)
 109     ;
 110
 111   bp_clear_buf(CPU_TX_BUF);
 112 }
 113
 114
 115 static size_t
 116 op_id_cmd(const op_generic_t *p,
 117           void *reply_payload, size_t reply_payload_space)
 118 {
 119   op_id_reply_t *r = (op_id_reply_t *) reply_payload;
 120   if (reply_payload_space < sizeof(*r)) // no room
 121     return 0;
 122
 123   // Build reply subpacket
 124
 125   r->opcode = OP_ID_REPLY;
 126   r->len = sizeof(op_id_reply_t);
 127   r->rid = p->rid;
 128   r->addr = *ethernet_mac_addr();
 129   r->hw_rev = (u2_hw_rev_major << 8) | u2_hw_rev_minor;
 130   // r->fpga_md5sum = ; // FIXME
 131   // r->sw_md5sum = ;   // FIXME
 132
 133   return r->len;
 134 }
 135
 136
 137 static size_t
 138 config_tx_v2_cmd(const op_config_tx_v2_t *p,
 139                  void *reply_payload, size_t reply_payload_space)
 140 {
 141   op_config_tx_reply_v2_t *r = (op_config_tx_reply_v2_t *) reply_payload;
 142   if (reply_payload_space < sizeof(*r))
 143     return 0;                                   // no room
 144
 145   struct tune_result    tune_result;
 146   memset(&tune_result, 0, sizeof(tune_result));
 147
 148   bool ok = true;
 149
 150   if (p->valid & CFGV_GAIN){
 151     ok &= db_set_gain(tx_dboard, p->gain);
 152   }
 153
 154   if (p->valid & CFGV_FREQ){
 155     bool was_streaming = is_streaming();
 156     if (was_streaming)
 157       stop_rx_cmd();
 158
 159     u2_fxpt_freq_t f = u2_fxpt_freq_from_hilo(p->freq_hi, p->freq_lo);
 160     bool tune_ok = db_tune(tx_dboard, f, &tune_result);
 161     ok &= tune_ok;
 162     print_tune_result("Tx", tune_ok, f, &tune_result);
 163
 164     if (was_streaming)
 165       restart_streaming();
 166   }
 167
 168   if (p->valid & CFGV_INTERP_DECIM){
 169     int interp = p->interp;
 170     int hb1 = 0;
 171     int hb2 = 0;
 172
 173     if (!(interp & 1)){
 174       hb2 = 1;
 175       interp = interp >> 1;
 176     }
 177
 178     if (!(interp & 1)){
 179       hb1 = 1;
 180       interp = interp >> 1;
 181     }
 182
 183     if (interp < MIN_CIC_INTERP || interp > MAX_CIC_INTERP)
 184       ok = false;
 185     else {
 186       dsp_tx_regs->interp_rate = (hb1<<9) | (hb2<<8) | interp;
 187       // printf("Interp: %d, register %d\n", p->interp, (hb1<<9) | (hb2<<8) | interp);
 188     }
 189   }
 190
 191   if (p->valid & CFGV_SCALE_IQ){
 192     dsp_tx_regs->scale_iq = p->scale_iq;
 193   }
 194
 195   // Build reply subpacket
 196
 197   r->opcode = OP_CONFIG_TX_REPLY_V2;
 198   r->len = sizeof(*r);
 199   r->rid = p->rid;
 200   r->ok = ok;
 201   r->inverted = tune_result.inverted;
 202   r->baseband_freq_hi = u2_fxpt_freq_hi(tune_result.baseband_freq);
 203   r->baseband_freq_lo = u2_fxpt_freq_lo(tune_result.baseband_freq);
 204   r->duc_freq_hi = u2_fxpt_freq_hi(tune_result.dxc_freq);
 205   r->duc_freq_lo = u2_fxpt_freq_lo(tune_result.dxc_freq);
 206   r->residual_freq_hi = u2_fxpt_freq_hi(tune_result.residual_freq);
 207   r->residual_freq_lo = u2_fxpt_freq_lo(tune_result.residual_freq);
 208   return r->len;
 209 }
 210
 211 static size_t
 212 config_rx_v2_cmd(const op_config_rx_v2_t *p,
 213                  void *reply_payload, size_t reply_payload_space)
 214 {
 215   op_config_rx_reply_v2_t *r = (op_config_rx_reply_v2_t *) reply_payload;
 216   if (reply_payload_space < sizeof(*r))
 217     return 0;                           // no room
 218
 219   struct tune_result    tune_result;
 220   memset(&tune_result, 0, sizeof(tune_result));
 221
 222   bool ok = true;
 223
 224   if (p->valid & CFGV_GAIN){
 225     ok &= db_set_gain(rx_dboard, p->gain);
 226   }
 227
 228   if (p->valid & CFGV_FREQ){
 229     bool was_streaming = is_streaming();
 230     if (was_streaming)
 231       stop_rx_cmd();
 232
 233     u2_fxpt_freq_t f = u2_fxpt_freq_from_hilo(p->freq_hi, p->freq_lo);
 234     bool tune_ok = db_tune(rx_dboard, f, &tune_result);
 235     ok &= tune_ok;
 236     print_tune_result("Rx", tune_ok, f, &tune_result);
 237
 238     if (was_streaming)
 239       restart_streaming();
 240   }
 241
 242   if (p->valid & CFGV_INTERP_DECIM){
 243     int decim = p->decim;
 244     int hb1 = 0;
 245     int hb2 = 0;
 246
 247     if(!(decim & 1)) {
 248       hb2 = 1;
 249       decim = decim >> 1;
 250     }
 251
 252     if(!(decim & 1)) {
 253       hb1 = 1;
 254       decim = decim >> 1;
 255     }
 256
 257     if (decim < MIN_CIC_DECIM || decim > MAX_CIC_DECIM)
 258       ok = false;
 259     else {
 260       dsp_rx_regs->decim_rate = (hb1<<9) | (hb2<<8) | decim;
 261       // printf("Decim: %d, register %d\n", p->decim, (hb1<<9) | (hb2<<8) | decim);
 262     }
 263   }
 264
 265   if (p->valid & CFGV_SCALE_IQ){
 266     dsp_rx_regs->scale_iq = p->scale_iq;
 267   }
 268
 269   // Build reply subpacket
 270
 271   r->opcode = OP_CONFIG_RX_REPLY_V2;
 272   r->len = sizeof(*r);
 273   r->rid = p->rid;
 274   r->ok = ok;
 275   r->inverted = tune_result.inverted;
 276   r->baseband_freq_hi = u2_fxpt_freq_hi(tune_result.baseband_freq);
 277   r->baseband_freq_lo = u2_fxpt_freq_lo(tune_result.baseband_freq);
 278   r->ddc_freq_hi = u2_fxpt_freq_hi(tune_result.dxc_freq);
 279   r->ddc_freq_lo = u2_fxpt_freq_lo(tune_result.dxc_freq);
 280   r->residual_freq_hi = u2_fxpt_freq_hi(tune_result.residual_freq);
 281   r->residual_freq_lo = u2_fxpt_freq_lo(tune_result.residual_freq);
 282
 283   return r->len;
 284 }
 285
 286 static size_t
 287 read_time_cmd(const op_generic_t *p,
 288               void *reply_payload, size_t reply_payload_space)
 289 {
 290   op_read_time_reply_t *r = (op_read_time_reply_t *) reply_payload;
 291   if (reply_payload_space < sizeof(*r))
 292     return 0;                                   // no room
 293
 294   r->opcode = OP_READ_TIME_REPLY;
 295   r->len = sizeof(*r);
 296   r->rid = p->rid;
 297   r->time = timer_regs->time;
 298
 299   return r->len;
 300 }
 301
 302 static void
 303 fill_db_info(u2_db_info_t *p, const struct db_base *db)
 304 {
 305   p->dbid = db->dbid;
 306   p->freq_min_hi = u2_fxpt_freq_hi(db->freq_min);
 307   p->freq_min_lo = u2_fxpt_freq_lo(db->freq_min);
 308   p->freq_max_hi = u2_fxpt_freq_hi(db->freq_max);
 309   p->freq_max_lo = u2_fxpt_freq_lo(db->freq_max);
 310   p->gain_min = db->gain_min;
 311   p->gain_max = db->gain_max;
 312   p->gain_step_size = db->gain_step_size;
 313 }
 314
 315 static size_t
 316 dboard_info_cmd(const op_generic_t *p,
 317                 void *reply_payload, size_t reply_payload_space)
 318 {
 319   op_dboard_info_reply_t *r = (op_dboard_info_reply_t *) reply_payload;
 320   if (reply_payload_space < sizeof(*r))
 321     return 0;                                   // no room
 322
 323   r->opcode = OP_DBOARD_INFO_REPLY;
 324   r->len = sizeof(*r);
 325   r->rid = p->rid;
 326   r->ok = true;
 327
 328   fill_db_info(&r->tx_db_info, tx_dboard);
 329   fill_db_info(&r->rx_db_info, rx_dboard);
 330
 331   return r->len;
 332 }
 333
 334 static size_t
 335 peek_cmd(const op_peek_t *p,
 336          void *reply_payload, size_t reply_payload_space)
 337 {
 338   op_generic_t *r = (op_generic_t *) reply_payload;
 339
 340   putstr("peek: addr="); puthex32(p->addr);
 341   printf(" bytes=%u\n", p->bytes);
 342
 343   if ((reply_payload_space < (sizeof(*r) + p->bytes)) ||
 344       p->bytes > MAX_SUBPKT_LEN - sizeof(op_generic_t)) {
 345     putstr("peek: insufficient reply packet space\n");
 346     return 0;                   // FIXME do partial read?
 347   }
 348
 349   r->opcode = OP_PEEK_REPLY;
 350   r->len = sizeof(*r)+p->bytes;
 351   r->rid = p->rid;
 352   r->ok = true;
 353
 354   memcpy_wa(reply_payload+sizeof(*r), (void *)p->addr, p->bytes);
 355
 356   return r->len;
 357 }
 358
 359 static bool
 360 poke_cmd(const op_poke_t *p)
 361 {
 362   int bytes = p->len - sizeof(*p);
 363   putstr("poke: addr="); puthex32(p->addr);
 364   printf(" bytes=%u\n", bytes);
 365
 366   uint8_t *src = (uint8_t *)p + sizeof(*p);
 367   memcpy_wa((void *)p->addr, src, bytes);
 368
 369   return true;
 370 }
 371
 372 static size_t
 373 generic_reply(const op_generic_t *p,
 374               void *reply_payload, size_t reply_payload_space,
 375               bool ok)
 376 {
 377   op_generic_t *r = (op_generic_t *) reply_payload;
 378   if (reply_payload_space < sizeof(*r))
 379     return 0;                                   // no room
 380
 381   r->opcode = p->opcode | OP_REPLY_BIT;
 382   r->len = sizeof(*r);
 383   r->rid = p->rid;
 384   r->ok = ok;
 385
 386   return r->len;
 387 }
 388
 389 static size_t
 390 add_eop(void *reply_payload, size_t reply_payload_space)
 391 {
 392   op_generic_t *r = (op_generic_t *) reply_payload;
 393   if (reply_payload_space < sizeof(*r))
 394     return 0;                                   // no room
 395
 396   r->opcode = OP_EOP;
 397   r->len = sizeof(*r);
 398   r->rid = 0;
 399   r->ok =  0;
 400
 401   return r->len;
 402 }
 403
 404 void
 405 handle_control_chan_frame(u2_eth_packet_t *pkt, size_t len)
 406 {
 407   unsigned char reply[sizeof(u2_eth_packet_t) + 4 * sizeof(u2_subpkt_t)] _AL4;
 408   unsigned char *reply_payload = &reply[sizeof(u2_eth_packet_t)];
 409   int reply_payload_space = sizeof(reply) - sizeof(u2_eth_packet_t);
 410
 411   // initialize reply
 412   memset(reply, 0, sizeof(reply));
 413   set_reply_hdr((u2_eth_packet_t *) reply, pkt);
 414
 415   // point to beginning of payload (subpackets)
 416   unsigned char *payload = ((unsigned char *) pkt) + sizeof(u2_eth_packet_t);
 417   int payload_len = len - sizeof(u2_eth_packet_t);
 418
 419   size_t subpktlen = 0;
 420
 421   while (payload_len >= sizeof(op_generic_t)){
 422     const op_generic_t *gp = (const op_generic_t *) payload;
 423     subpktlen = 0;
 424
 425     // printf("\nopcode = %d\n", gp->opcode);
 426
 427     switch(gp->opcode){
 428     case OP_EOP:                // end of subpackets
 429       goto end_of_subpackets;
 430
 431     case OP_ID:
 432       subpktlen = op_id_cmd(gp, reply_payload, reply_payload_space);
 433       break;
 434
 435     case OP_CONFIG_TX_V2:
 436       subpktlen = config_tx_v2_cmd((op_config_tx_v2_t *) payload,
 437                                    reply_payload, reply_payload_space);
 438       break;
 439
 440     case OP_CONFIG_RX_V2:
 441       subpktlen = config_rx_v2_cmd((op_config_rx_v2_t *) payload,
 442                                    reply_payload, reply_payload_space);
 443       break;
 444
 445     case OP_START_RX_STREAMING:
 446       start_rx_streaming_cmd(&pkt->ehdr.src, (op_start_rx_streaming_t *) payload);
 447       subpktlen = generic_reply(gp, reply_payload, reply_payload_space, true);
 448       break;
 449
 450     case OP_STOP_RX:
 451       stop_rx_cmd();
 452       subpktlen = generic_reply(gp, reply_payload, reply_payload_space, true);
 453       break;
 454
 455     case OP_BURN_MAC_ADDR:
 456       subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
 457                                 burn_mac_addr((op_burn_mac_addr_t *) payload));
 458       break;
 459
 460     case OP_CONFIG_MIMO:
 461       subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
 462                                 config_mimo_cmd((op_config_mimo_t *) payload));
 463       break;
 464
 465     case OP_READ_TIME:
 466       subpktlen = read_time_cmd(gp, reply_payload, reply_payload_space);
 467       break;
 468
 469     case OP_DBOARD_INFO:
 470       subpktlen = dboard_info_cmd(gp, reply_payload, reply_payload_space);
 471       break;
 472
 473     case OP_SYNC_TO_PPS:
 474       subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
 475                                 sync_to_pps((op_generic_t *) payload));
 476       break;
 477
 478     case OP_PEEK:
 479       subpktlen = peek_cmd((op_peek_t *)payload, reply_payload, reply_payload_space);
 480       break;
 481
 482     case OP_POKE:
 483       subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
 484                                 poke_cmd((op_poke_t *)payload));
 485       break;
 486
 487     default:
 488       printf("app_common_v2: unhandled opcode = %d\n", gp->opcode);
 489       break;
 490     }
 491
 492     int t = (gp->len + 3) & ~3;         // bump to a multiple of 4
 493     payload += t;
 494     payload_len -= t;
 495
 496     subpktlen = (subpktlen + 3) & ~3;   // bump to a multiple of 4
 497     reply_payload += subpktlen;
 498     reply_payload_space -= subpktlen;
 499   }
 500
 501  end_of_subpackets:
 502
 503   // add the EOP marker
 504   subpktlen = add_eop(reply_payload, reply_payload_space);
 505   subpktlen = (subpktlen + 3) & ~3;     // bump to a multiple of 4
 506   reply_payload += subpktlen;
 507   reply_payload_space -= subpktlen;
 508
 509   send_reply(reply, reply_payload - reply);
 510 }
 511
 512
 513 /*
 514  * Called when an ethernet packet is received.
 515  * Return true if we handled it here, otherwise
 516  * it'll be passed on to the DSP Tx pipe
 517  */
 518 bool
 519 eth_pkt_inspector(dbsm_t *sm, int bufno)
 520 {
 521   u2_eth_packet_t *pkt = (u2_eth_packet_t *) buffer_ram(bufno);
 522   size_t byte_len = (buffer_pool_status->last_line[bufno] - 3) * 4;
 523
 524   //static size_t last_len = 0;
 525
 526   // hal_toggle_leds(0x1);
 527
 528   // inspect rcvd frame and figure out what do do.
 529
 530   if (pkt->ehdr.ethertype != U2_ETHERTYPE)
 531     return true;        // ignore, probably bogus PAUSE frame from MAC
 532
 533   int chan = u2p_chan(&pkt->fixed);
 534
 535   switch (chan){
 536   case CONTROL_CHAN:
 537     handle_control_chan_frame(pkt, byte_len);
 538     return true;        // we handled the packet
 539     break;
 540
 541   case 0:
 542   default:
 543 #if 0
 544     if (last_len != 0){
 545       if (byte_len != last_len){
 546         printf("Len: %d last: %d\n", byte_len, last_len);
 547       }
 548     }
 549     last_len = byte_len;
 550
 551     if((pkt->thdr.seqno) == exp_seqno){
 552       exp_seqno++;
 553       //putchar('.');
 554     }
 555     else {
 556       // putchar('S');
 557       //printf("S%d %d ",exp_seqno,pkt->thdr.seqno);
 558       exp_seqno = pkt->thdr.seqno + 1;
 559     }
 560 #endif
 561     return false;       // pass it on to Tx DSP
 562     break;
 563   }
 564 }
 565
 566 /*
 567  * Called when eth phy state changes (w/ interrupts disabled)
 568  */
 569 void
 570 link_changed_callback(int speed)
 571 {
 572   link_is_up = speed != 0;
 573   hal_set_leds(link_is_up ? LED_RJ45 : 0x0, LED_RJ45);
 574   printf("\neth link changed: speed = %d\n", speed);
 575 }
 576
 577
 578 void
 579 print_tune_result(char *msg, bool tune_ok,
 580                   u2_fxpt_freq_t target_freq, struct tune_result *r)
 581 {
 582 #if 0
 583   printf("db_tune %s %s\n", msg, tune_ok ? "true" : "false");
 584   putstr("  target_freq   "); print_fxpt_freq(target_freq); newline();
 585   putstr("  baseband_freq "); print_fxpt_freq(r->baseband_freq); newline();
 586   putstr("  dxc_freq      "); print_fxpt_freq(r->dxc_freq); newline();
 587   putstr("  residual_freq "); print_fxpt_freq(r->residual_freq); newline();
 588   printf("  inverted      %s\n", r->inverted ? "true" : "false");
 589 #endif
 590 }