← Back to branch summary

~ubuntu-branches/ubuntu/trusty/gnuradio/trusty-updates

« back to all changes in this revision

Viewing changes to usrp2/firmware/apps/app_common_v2.c

  • Committer: Package Import Robot
  • Author(s): A. Maitland Bottoms
  • Date: 2012-02-26 21:26:16 UTC
  • mfrom: (1.1.4)
  • Revision ID: package-import@ubuntu.com-20120226212616-vsfkbi1158xshdql
Tags: 3.5.1-1
http://bugs.debian.org/642716
http://bugs.debian.org/645332
http://bugs.debian.org/394849
http://bugs.debian.org/616832
http://bugs.debian.org/590048
http://bugs.debian.org/642580
* new upstream version, re-packaged from scratch with modern tools
    closes: #642716, #645332, #394849, #616832, #590048, #642580,
    #647018, #557050, #559640, #631863
* CMake build

Show diffs side-by-side

added added

removed removed

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