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

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