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