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