git.gag.com Git - debian/gnuradio/blob - usrp/host/lib/inband/usrp_inband_usb_packet.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2007 Free Software Foundation, Inc.
   4  *
   5  * This file is part of GNU Radio
   6  *
   7  * GNU Radio is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 3, or (at your option)
  10  * any later version.
  11  *
  12  * GNU Radio is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License along
  18  * with this program; if not, write to the Free Software Foundation, Inc.,
  19  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include <config.h>
  24 #endif
  25
  26 #include <usrp_inband_usb_packet.h>
  27
  28 #include <usrp_bytesex.h>
  29 #include <iostream>
  30 #include <stdio.h>
  31 #include <string.h>
  32
  33 bool usrp_inband_usb_packet::align32()
  34 {
  35   int p_len = payload_len();
  36
  37   int bytes_needed = 4 - (p_len % 4);
  38
  39   if(bytes_needed == 4)
  40     return true;
  41
  42   // If the room left in the packet is less than the number of bytes
  43   // needed, return false to indicate no room to align
  44   if((MAX_PAYLOAD - p_len) < bytes_needed)
  45     return false;
  46
  47   p_len += bytes_needed;
  48
  49   int h_flags = flags();
  50   int h_chan = chan();
  51   int h_tag = tag();
  52   int h_payload_len = p_len;
  53
  54   set_header(h_flags, h_chan, h_tag, h_payload_len);
  55
  56   return true;
  57 }
  58
  59 bool usrp_inband_usb_packet::cs_ping(long rid, long ping_val)
  60 {
  61   if(!align32())
  62     return false;
  63
  64   int p_len = payload_len();
  65
  66   if((MAX_PAYLOAD - p_len) < (CS_PING_LEN + CS_FIXED_LEN))
  67     return false;
  68
  69   uint32_t ping = (
  70       ((OP_PING_FIXED & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
  71     | ((CS_PING_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
  72     | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
  73     | (ping_val & CS_PINGVAL_MASK)
  74
  75     );
  76
  77   uint32_t *payload = (uint32_t *) (d_payload + p_len);
  78   *payload = host_to_usrp_u32(ping);
  79
  80   // Update payload length
  81   int h_flags = flags();
  82   int h_chan = chan();
  83   int h_tag = tag();
  84   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_PING_LEN;
  85
  86   set_header(h_flags, h_chan, h_tag, h_payload_len);
  87
  88   return true;
  89 }
  90
  91
  92 bool usrp_inband_usb_packet::cs_ping_reply(long rid, long ping_val)
  93 {
  94   if(!align32())
  95     return false;
  96
  97   int p_len = payload_len();
  98
  99   if((MAX_PAYLOAD - p_len) < (CS_PING_LEN + CS_FIXED_LEN))
 100     return false;
 101
 102   uint32_t ping = (
 103       ((OP_PING_FIXED_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 104     | ((CS_PING_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
 105     | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
 106     | ((ping_val & CS_PINGVAL_MASK) << CS_PINGVAL_SHIFT)
 107
 108     );
 109
 110   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 111   *payload = host_to_usrp_u32(ping);
 112
 113   // Update payload length
 114   int h_flags = flags();
 115   int h_chan = chan();
 116   int h_tag = tag();
 117   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_PING_LEN;
 118
 119   set_header(h_flags, h_chan, h_tag, h_payload_len);
 120
 121   return true;
 122 }
 123
 124 bool usrp_inband_usb_packet::cs_write_reg(long reg_num, long val)
 125 {
 126   if(!align32())
 127     return false;
 128
 129   int p_len = payload_len();
 130
 131   if((MAX_PAYLOAD - p_len) < (CS_WRITEREG_LEN + CS_FIXED_LEN))
 132     return false;
 133
 134   uint32_t word0 = 0;
 135
 136   // Build the first word which includes the register number
 137   word0 = (
 138       ((OP_WRITE_REG & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 139     | ((CS_WRITEREG_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
 140     | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT)
 141   );
 142
 143   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 144   *payload = host_to_usrp_u32(word0);
 145
 146   // The second word is solely the register value to be written
 147   // FIXME: should this be unsigned?
 148   payload += 1;
 149   *payload = host_to_usrp_u32((uint32_t) val);
 150
 151   // Rebuild the header to update the payload length
 152   int h_flags = flags();
 153   int h_chan = chan();
 154   int h_tag = tag();
 155   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_WRITEREG_LEN;
 156
 157   set_header(h_flags, h_chan, h_tag, h_payload_len);
 158
 159   return true;
 160 }
 161
 162 bool usrp_inband_usb_packet::cs_write_reg_masked(long reg_num, long val, long mask)
 163 {
 164   if(!align32())
 165     return false;
 166
 167   int p_len = payload_len();
 168
 169   if((MAX_PAYLOAD - p_len) < (CS_WRITEREGMASKED_LEN + CS_FIXED_LEN))
 170     return false;
 171
 172   uint32_t word0 = 0;
 173
 174   // Build the first word which includes the register number
 175   word0 = (
 176       ((OP_WRITE_REG_MASKED & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 177     | ((CS_WRITEREGMASKED_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
 178     | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT)
 179   );
 180
 181   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 182   *payload = host_to_usrp_u32(word0);
 183
 184   // Skip over the first word and write the register value
 185   payload += 1;
 186   *payload = host_to_usrp_u32((uint32_t) val);
 187
 188   // Skip over the register value and write the mask
 189   payload += 1;
 190   *payload = host_to_usrp_u32((uint32_t) mask);
 191
 192   // Rebuild the header to update the payload length
 193   int h_flags = flags();
 194   int h_chan = chan();
 195   int h_tag = tag();
 196   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_WRITEREGMASKED_LEN;
 197
 198   set_header(h_flags, h_chan, h_tag, h_payload_len);
 199
 200   return true;
 201 }
 202
 203 bool usrp_inband_usb_packet::cs_read_reg(long rid, long reg_num)
 204 {
 205   if(!align32())
 206     return false;
 207
 208   int p_len = payload_len();
 209
 210   if((MAX_PAYLOAD - p_len) < (CS_READREG_LEN + CS_FIXED_LEN))
 211     return false;
 212
 213   uint32_t read_reg = (
 214       ((OP_READ_REG & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 215     | ((CS_READREG_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
 216     | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
 217     | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT)
 218
 219     );
 220
 221   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 222   *payload = host_to_usrp_u32(read_reg);
 223
 224   // Update payload length
 225   int h_flags = flags();
 226   int h_chan = chan();
 227   int h_tag = tag();
 228   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_READREG_LEN;
 229
 230   set_header(h_flags, h_chan, h_tag, h_payload_len);
 231
 232   return true;
 233 }
 234
 235 bool usrp_inband_usb_packet::cs_read_reg_reply(long rid, long reg_num, long reg_val)
 236 {
 237   if(!align32())
 238     return false;
 239
 240   int p_len = payload_len();
 241
 242   if((MAX_PAYLOAD - p_len) < (CS_READREGREPLY_LEN + CS_FIXED_LEN))
 243     return false;
 244
 245   uint32_t word0 = (
 246       ((OP_READ_REG_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 247     | ((CS_READREGREPLY_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
 248     | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
 249     | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT)
 250
 251     );
 252
 253   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 254   *payload = host_to_usrp_u32(word0);
 255
 256   // Hop to the next word and write the reg value
 257   payload += 1;
 258   *payload = host_to_usrp_u32((uint32_t) reg_val);
 259
 260   // Update payload length
 261   int h_flags = flags();
 262   int h_chan = chan();
 263   int h_tag = tag();
 264   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_READREGREPLY_LEN;
 265
 266   set_header(h_flags, h_chan, h_tag, h_payload_len);
 267
 268   return true;
 269 }
 270
 271 bool usrp_inband_usb_packet::cs_delay(long ticks)
 272 {
 273   if(!align32())
 274     return false;
 275
 276   int p_len = payload_len();
 277
 278   if((MAX_PAYLOAD - p_len) < (CS_DELAY_LEN + CS_FIXED_LEN))
 279     return false;
 280
 281   uint32_t delay = (
 282       ((OP_DELAY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 283     | ((CS_DELAY_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
 284     | ((ticks & CS_DELAY_MASK) << CS_DELAY_SHIFT)
 285
 286     );
 287
 288   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 289   *payload = host_to_usrp_u32(delay);
 290
 291   // Update payload length
 292   int h_flags = flags();
 293   int h_chan = chan();
 294   int h_tag = tag();
 295   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_DELAY_LEN;
 296
 297   set_header(h_flags, h_chan, h_tag, h_payload_len);
 298
 299   return true;
 300 }
 301
 302 bool usrp_inband_usb_packet::cs_i2c_write(long i2c_addr, uint8_t *i2c_data, size_t data_len)
 303 {
 304   if(!align32())
 305     return false;
 306
 307   int p_len = payload_len();
 308
 309   int i2c_len = data_len + 2;   // 2 bytes between mbz and addr
 310
 311   if((MAX_PAYLOAD - p_len) < (i2c_len + CS_FIXED_LEN))
 312     return false;
 313
 314   uint32_t word0 = 0;
 315
 316   word0 = (
 317       ((OP_I2C_WRITE & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 318     | ((i2c_len & CS_LEN_MASK) << CS_LEN_SHIFT)
 319     | ((i2c_addr & CS_I2CADDR_MASK) << CS_I2CADDR_SHIFT)
 320   );
 321
 322   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 323    *payload = host_to_usrp_u32(word0);
 324
 325    // Jump over the first word and write the data
 326    // FIXME: Should the data be changed to usrp byte order?
 327    payload += 1;
 328    memcpy(payload, i2c_data, data_len);
 329
 330   // Update payload length
 331   int h_flags = flags();
 332   int h_chan = chan();
 333   int h_tag = tag();
 334   int h_payload_len = payload_len() + CS_FIXED_LEN + i2c_len;
 335
 336   set_header(h_flags, h_chan, h_tag, h_payload_len);
 337
 338   return true;
 339 }
 340
 341 bool usrp_inband_usb_packet::cs_i2c_read(long rid, long i2c_addr, long n_bytes)
 342 {
 343   if(!align32())
 344     return false;
 345
 346   int p_len = payload_len();
 347
 348   if((MAX_PAYLOAD - p_len) < (CS_I2CREAD_LEN + CS_FIXED_LEN))
 349     return false;
 350
 351   uint32_t word0 = 0;
 352
 353   word0 = (
 354       ((OP_I2C_READ & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 355     | ((CS_I2CREAD_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
 356     | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
 357     | ((i2c_addr & CS_I2CADDR_MASK) << CS_I2CADDR_SHIFT)
 358     );
 359
 360   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 361   *payload = host_to_usrp_u32(word0);
 362
 363   // Jump a word and write the number of bytes to read
 364   payload += 1;
 365   uint32_t word1 =
 366     (n_bytes & CS_I2CREADBYTES_MASK) << CS_I2CREADBYTES_SHIFT;
 367   *payload = host_to_usrp_u32(word1);
 368
 369   // Update payload length
 370   int h_flags = flags();
 371   int h_chan = chan();
 372   int h_tag = tag();
 373   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_I2CREAD_LEN;
 374
 375   set_header(h_flags, h_chan, h_tag, h_payload_len);
 376
 377   return true;
 378 }
 379
 380 bool usrp_inband_usb_packet::cs_i2c_read_reply(long rid, long i2c_addr, uint8_t *i2c_data, long i2c_data_len)
 381 {
 382   if(!align32())
 383     return false;
 384
 385   int p_len = payload_len();
 386
 387   int i2c_len = i2c_data_len + 2;
 388
 389   if((MAX_PAYLOAD - p_len) < (i2c_len + CS_FIXED_LEN))
 390     return false;
 391
 392   uint32_t word0 = 0;
 393
 394   word0 = (
 395       ((OP_I2C_READ_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 396     | ((i2c_len & CS_LEN_MASK) << CS_LEN_SHIFT)
 397     | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
 398     | ((i2c_addr & CS_I2CADDR_MASK) << CS_I2CADDR_SHIFT)
 399     );
 400
 401   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 402   *payload = host_to_usrp_u32(word0);
 403
 404   // Jump a word and write the actual data
 405   payload += 1;
 406   memcpy(payload, i2c_data, i2c_data_len);
 407
 408   // Update payload length
 409   int h_flags = flags();
 410   int h_chan = chan();
 411   int h_tag = tag();
 412   int h_payload_len = payload_len() + CS_FIXED_LEN + i2c_len;
 413
 414   set_header(h_flags, h_chan, h_tag, h_payload_len);
 415
 416   return true;
 417 }
 418
 419 bool usrp_inband_usb_packet::cs_spi_write(long enables, long format, long opt_header_bytes, uint8_t *spi_data, long spi_data_len)
 420 {
 421   if(!align32())
 422     return false;
 423
 424   int p_len = payload_len();
 425
 426   int spi_len = spi_data_len + 6;
 427
 428   if((MAX_PAYLOAD - p_len) < (spi_len + CS_FIXED_LEN))
 429     return false;
 430
 431   uint32_t word = 0;
 432
 433   // First word contains the opcode and length, then mbz
 434   word = (
 435       ((OP_SPI_WRITE & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 436     | ((spi_len & CS_LEN_MASK) << CS_LEN_SHIFT)
 437     );
 438   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 439   *payload = host_to_usrp_u32(word);
 440
 441   payload += 1;
 442
 443   // Second word contains the enables, format, and optional tx bytes
 444   word = 0;
 445   word = (
 446       ((enables & CS_SPIENABLES_MASK) << CS_SPIENABLES_SHIFT)
 447     | ((format & CS_SPIFORMAT_MASK) << CS_SPIFORMAT_SHIFT)
 448     | ((opt_header_bytes & CS_SPIOPT_MASK) << CS_SPIOPT_SHIFT)
 449     );
 450   payload = (uint32_t *) (d_payload + p_len);
 451   *payload = host_to_usrp_u32(word);
 452
 453   payload += 1;
 454   memcpy(payload, spi_data, spi_data_len);
 455
 456   // Update payload length
 457   int h_flags = flags();
 458   int h_chan = chan();
 459   int h_tag = tag();
 460   int h_payload_len = payload_len() + CS_FIXED_LEN + spi_len;
 461
 462   set_header(h_flags, h_chan, h_tag, h_payload_len);
 463
 464   return true;
 465 }
 466
 467 bool usrp_inband_usb_packet::cs_spi_read(long rid, long enables, long format, long opt_header_bytes, long n_bytes)
 468 {
 469   if(!align32())
 470     return false;
 471
 472   int p_len = payload_len();
 473
 474   if((MAX_PAYLOAD - p_len) < (CS_SPIREAD_LEN + CS_FIXED_LEN))
 475     return false;
 476
 477   uint32_t word = 0;
 478
 479   // First word contains the opcode, length, and RID
 480   word = (
 481       ((OP_SPI_READ & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 482     | ((CS_SPIREAD_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
 483     | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
 484     );
 485   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 486   *payload = host_to_usrp_u32(word);
 487
 488   payload += 1;
 489
 490   // Second word contains the enables, format, and optional tx bytes
 491   word = 0;
 492   word = (
 493       ((enables & CS_SPIENABLES_MASK) << CS_SPIENABLES_SHIFT)
 494     | ((format & CS_SPIFORMAT_MASK) << CS_SPIFORMAT_SHIFT)
 495     | ((opt_header_bytes & CS_SPIOPT_MASK) << CS_SPIOPT_SHIFT)
 496     );
 497   payload = (uint32_t *) (d_payload + p_len);
 498   *payload = host_to_usrp_u32(word);
 499
 500   payload += 1;
 501
 502   // The third word contains the number of bytes
 503   word = 0;
 504   word = (
 505       ((n_bytes & CS_SPINBYTES_MASK) << CS_SPINBYTES_SHIFT)
 506     );
 507   payload = (uint32_t *) (d_payload + p_len);
 508   *payload = host_to_usrp_u32(word);
 509
 510   // Update payload length
 511   int h_flags = flags();
 512   int h_chan = chan();
 513   int h_tag = tag();
 514   int h_payload_len = payload_len() + CS_FIXED_LEN + CS_SPIREAD_LEN;
 515
 516   set_header(h_flags, h_chan, h_tag, h_payload_len);
 517
 518   return true;
 519 }
 520
 521 bool usrp_inband_usb_packet::cs_spi_read_reply(long rid, uint8_t *spi_data, long spi_data_len)
 522 {
 523   if(!align32())
 524     return false;
 525
 526   int p_len = payload_len();
 527
 528   int spi_len = spi_data_len + 2;
 529
 530   if((MAX_PAYLOAD - p_len) < (spi_len + CS_FIXED_LEN))
 531     return false;
 532
 533   uint32_t word = 0;
 534
 535   // First word contains the opcode, length, and RID
 536   word = (
 537       ((OP_SPI_READ_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
 538     | ((spi_len & CS_LEN_MASK) << CS_LEN_SHIFT)
 539     | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
 540     );
 541   uint32_t *payload = (uint32_t *) (d_payload + p_len);
 542   *payload = host_to_usrp_u32(word);
 543
 544   // Jump a word and write the actual data
 545   payload += 1;
 546   memcpy(payload, spi_data, spi_data_len);
 547
 548   // Update payload length
 549   int h_flags = flags();
 550   int h_chan = chan();
 551   int h_tag = tag();
 552   int h_payload_len = payload_len() + CS_FIXED_LEN + spi_len;
 553
 554   set_header(h_flags, h_chan, h_tag, h_payload_len);
 555
 556   return true;
 557 }
 558
 559 // Takes an offset to the beginning of a subpacket and extracts the
 560 // length of the subpacket
 561 int usrp_inband_usb_packet::cs_len(int payload_offset) {
 562   uint32_t subpkt = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset)));
 563   return (subpkt >> CS_LEN_SHIFT) & CS_LEN_MASK;
 564 }
 565
 566 // The following method takes an offset within the packet payload to extract
 567 // a control/status subpacket and construct a pmt response which includes the
 568 // proper signal and arguments specified by usrp-low-level-cs.  The USRP
 569 // server could therefore use this to read subpackets and pass them responses
 570 // back up to the application.  It's arguable that only reply packets should
 571 // be parsed here, however we parse others for use in debugging or failure
 572 // reporting on the transmit side of packets.
 573 pmt_t usrp_inband_usb_packet::read_subpacket(int payload_offset) {
 574
 575   uint32_t subpkt = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset)));
 576   uint32_t opcode = (subpkt >> CS_OPCODE_SHIFT) & CS_OPCODE_MASK;
 577   uint32_t len = (subpkt >> CS_LEN_SHIFT) & CS_LEN_MASK;
 578
 579   switch(opcode) {
 580
 581     case OP_PING_FIXED_REPLY:
 582     {
 583       pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
 584       pmt_t pingval = pmt_from_long((subpkt >> CS_PINGVAL_SHIFT) & CS_PINGVAL_MASK);
 585       return pmt_list3(s_op_ping_fixed_reply, rid, pingval);
 586     }
 587
 588     case OP_READ_REG_REPLY:
 589     {
 590       pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
 591       pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & CS_REGNUM_MASK);
 592
 593       // To get the register value we just read the next 32 bits
 594       uint32_t val  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4)));
 595       pmt_t reg_val = pmt_from_long(val);
 596
 597       return pmt_list4(s_op_read_reg_reply, rid, reg_num, reg_val);
 598     }
 599
 600     case OP_I2C_READ_REPLY:
 601     {
 602       pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
 603       pmt_t i2c_addr  = pmt_from_long((subpkt >> CS_I2CADDR_SHIFT) & CS_I2CADDR_MASK);
 604
 605       // Make a u8 vector to dump the data from the packet into
 606       size_t i2c_data_len;
 607       pmt_t i2c_data  = pmt_make_u8vector(len - 2, 0);   // skip rid+mbz+addr = 2 bytes
 608       uint8_t *w_data  =
 609           (uint8_t *) pmt_u8vector_writeable_elements(i2c_data, i2c_data_len);
 610
 611       memcpy(w_data, d_payload + payload_offset + 4, i2c_data_len);  // skip first word
 612
 613       return pmt_list4(s_op_i2c_read_reply, rid, i2c_addr, i2c_data);
 614     }
 615
 616     case OP_SPI_READ_REPLY:
 617     {
 618       pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
 619
 620       // Make a u8 vector to dump the data from the packet into
 621       size_t spi_data_len;
 622       pmt_t spi_data  = pmt_make_u8vector(len - 2, 0);   // skip rid+mbz+addr = 2 bytes
 623       uint8_t *w_data  =
 624           (uint8_t *) pmt_u8vector_writeable_elements(spi_data, spi_data_len);
 625
 626       memcpy(w_data, d_payload + payload_offset + 4, spi_data_len);  // skip first word
 627
 628       return pmt_list3(s_op_spi_read_reply, rid, spi_data);
 629     }
 630
 631     case OP_PING_FIXED:
 632     {
 633       pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
 634       pmt_t pingval = pmt_from_long((subpkt >> CS_PINGVAL_SHIFT) & CS_PINGVAL_MASK);
 635       return pmt_list3(s_op_ping_fixed, rid, pingval);
 636     }
 637
 638     case OP_WRITE_REG:
 639     {
 640       pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & CS_REGNUM_MASK);
 641
 642       // To get the register value we just read the next 32 bits
 643       uint32_t val  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4)));
 644       pmt_t reg_val = pmt_from_long(val);
 645
 646       return pmt_list3(s_op_write_reg, reg_num, reg_val);
 647     }
 648
 649     case OP_WRITE_REG_MASKED:
 650     {
 651       pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & CS_REGNUM_MASK);
 652
 653       // To get the register value we just read the next 32 bits
 654       uint32_t val  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4)));
 655       pmt_t reg_val = pmt_from_long(val);
 656
 657       // The mask is the next 32 bits
 658       uint32_t mask  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 8)));
 659       pmt_t reg_mask = pmt_from_long(mask);
 660
 661       return pmt_list4(s_op_write_reg_masked, reg_num, reg_val, reg_mask);
 662     }
 663
 664     case OP_READ_REG:
 665     {
 666       pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
 667       pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & CS_REGNUM_MASK);
 668
 669       return pmt_list3(s_op_read_reg, rid, reg_num);
 670     }
 671
 672     case OP_I2C_WRITE:
 673     {
 674       pmt_t i2c_addr    = pmt_from_long((subpkt >> CS_I2CADDR_SHIFT) & CS_I2CADDR_MASK);
 675
 676       // The length includes an extra 2 bytes for storing the mbz and addr
 677       pmt_t i2c_data    = pmt_make_u8vector(len-2, 0);
 678
 679       // Get a writeable address to copy the data from the packet
 680       size_t ignore;
 681       uint8_t *w_data = (uint8_t *) pmt_u8vector_writeable_elements(i2c_data, ignore);
 682       memcpy(w_data, d_payload + payload_offset + 4, len-2);
 683
 684
 685       return pmt_list3(s_op_i2c_write, i2c_addr, i2c_data);
 686     }
 687
 688     case OP_I2C_READ:
 689     {
 690       pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
 691       pmt_t i2c_addr  = pmt_from_long((subpkt >> CS_I2CADDR_SHIFT) & CS_I2CADDR_MASK);
 692
 693       // The number of bytes is in the next word
 694       uint32_t bytes  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4)));
 695       bytes = (bytes >> CS_I2CREADBYTES_SHIFT) & CS_I2CREADBYTES_MASK;
 696       pmt_t i2c_bytes = pmt_from_long(bytes);
 697
 698       return pmt_list4(s_op_i2c_read, rid, i2c_addr, i2c_bytes);
 699     }
 700
 701     case OP_SPI_WRITE:
 702     {
 703       // Nothing interesting in the first word, skip to the next
 704       uint32_t word  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4)));
 705       pmt_t enables   = pmt_from_long((word >> CS_SPIENABLES_SHIFT) & CS_SPIENABLES_MASK);
 706       pmt_t format    = pmt_from_long((word >> CS_SPIFORMAT_SHIFT) & CS_SPIFORMAT_MASK);
 707       pmt_t opt       = pmt_from_long((word >> CS_SPIOPT_SHIFT) & CS_SPIOPT_MASK);
 708
 709       // From the next word and on is data
 710       size_t spi_data_len;
 711       pmt_t spi_data  = pmt_make_u8vector(len - 6, 0);   // skip rid+mbz+addr = 2 bytes
 712       uint8_t *w_data  =
 713           (uint8_t *) pmt_u8vector_writeable_elements(spi_data, spi_data_len);
 714
 715       memcpy(w_data, d_payload + payload_offset + 8, spi_data_len);  // skip first 2 words
 716
 717       return pmt_list5(s_op_spi_write, enables, format, opt, spi_data);
 718     }
 719
 720     case OP_SPI_READ:
 721     {
 722       // Read the RID from the first word, the rest is mbz
 723       pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
 724
 725       // Continue at the next word...
 726       uint32_t word  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4)));
 727       pmt_t enables   = pmt_from_long((word >> CS_SPIENABLES_SHIFT) & CS_SPIENABLES_MASK);
 728       pmt_t format    = pmt_from_long((word >> CS_SPIFORMAT_SHIFT) & CS_SPIFORMAT_MASK);
 729       pmt_t opt       = pmt_from_long((word >> CS_SPIOPT_SHIFT) & CS_SPIOPT_MASK);
 730
 731       // The number of bytes is the only thing to read in the next word
 732       word  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 8)));
 733       pmt_t n_bytes   = pmt_from_long((word >> CS_SPINBYTES_SHIFT) & CS_SPINBYTES_MASK);
 734
 735       return pmt_list6(s_op_spi_read, rid, enables, format, opt, n_bytes);
 736     }
 737
 738     case OP_DELAY:
 739     {
 740       pmt_t ticks = pmt_from_long((subpkt >> CS_DELAY_SHIFT) & CS_DELAY_MASK);
 741
 742       return pmt_list2(s_op_delay, ticks);
 743     }
 744
 745     default:
 746       return PMT_NIL;
 747
 748   }
 749 }
 750