Changeset 6075

Show
Ignore:
Timestamp:
07/26/07 11:13:23
Author:
gnychis
Message:

Ripping the C/S subpacket creation and reading code out of the .h and in to a
new cc file.

Files:

Legend:

Unmodified
Added
Removed
Modified
Copied
Moved

  • gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/Makefile.am

    r5753 r6075  
    5353        $(BUILT_SOURCES)                \ 
    5454        ../../apps/ui_sincos.c          \ 
     55        usrp_inband_usb_packet.cc               \ 
    5556        usrp_rx.cc                      \ 
    5657        usrp_rx_stub.cc                 \ 

  • gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_inband_usb_packet.h

    r6023 r6075  
    213213  } 
    214214 
    215   bool align32() 
    216   { 
    217     int p_len = payload_len(); 
    218  
    219     int bytes_needed = 4 - (p_len % 4); 
    220  
    221     if(bytes_needed == 4) 
    222       return true; 
    223  
    224     // If the room left in the packet is less than the number of bytes 
    225     // needed, return false to indicate no room to align 
    226     if((MAX_PAYLOAD - p_len) < bytes_needed) 
    227       return false; 
    228        
    229     p_len += bytes_needed; 
    230    
    231     int h_flags = flags(); 
    232     int h_chan = chan(); 
    233     int h_tag = tag(); 
    234     int h_payload_len = p_len; 
    235  
    236     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    237  
    238     return true; 
    239   } 
    240  
    241   bool cs_ping(long rid, long ping_val) 
    242   { 
    243     if(!align32()) 
    244       return false; 
    245      
    246     int p_len = payload_len(); 
    247  
    248     if((MAX_PAYLOAD - p_len) < (CS_PING_LEN + CS_FIXED_LEN)) 
    249       return false; 
    250  
    251     uint32_t ping = (  
    252         ((OP_PING_FIXED & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    253       | ((CS_PING_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    254       | ((rid & CS_RID_MASK) << CS_RID_SHIFT) 
    255       | (ping_val & CS_PINGVAL_MASK) 
    256  
    257       ); 
    258  
    259     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    260     *payload = host_to_usrp_u32(ping); 
    261  
    262     // Update payload length 
    263     int h_flags = flags(); 
    264     int h_chan = chan(); 
    265     int h_tag = tag(); 
    266     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_PING_LEN; 
    267  
    268     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    269  
    270     return true; 
    271   } 
    272    
    273   bool cs_ping_reply(long rid, long ping_val)  
    274   { 
    275     if(!align32()) 
    276       return false; 
    277  
    278     int p_len = payload_len(); 
    279  
    280     if((MAX_PAYLOAD - p_len) < (CS_PING_LEN + CS_FIXED_LEN)) 
    281       return false; 
    282  
    283     uint32_t ping = (  
    284         ((OP_PING_FIXED_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    285       | ((CS_PING_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    286       | ((rid & CS_RID_MASK) << CS_RID_SHIFT) 
    287       | ((ping_val & CS_PINGVAL_MASK) << CS_PINGVAL_SHIFT) 
    288  
    289       ); 
    290  
    291     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    292     *payload = host_to_usrp_u32(ping); 
    293  
    294     // Update payload length 
    295     int h_flags = flags(); 
    296     int h_chan = chan(); 
    297     int h_tag = tag(); 
    298     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_PING_LEN; 
    299  
    300     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    301  
    302     return true; 
    303   } 
    304  
    305   bool cs_write_reg(long reg_num, long val) 
    306   { 
    307     if(!align32()) 
    308       return false; 
    309  
    310     int p_len = payload_len(); 
    311  
    312     if((MAX_PAYLOAD - p_len) < (CS_WRITEREG_LEN + CS_FIXED_LEN)) 
    313       return false; 
    314  
    315     uint32_t word0 = 0; 
    316  
    317     // Build the first word which includes the register number 
    318     word0 = ( 
    319         ((OP_WRITE_REG & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    320       | ((CS_WRITEREG_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    321       | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT) 
    322     ); 
    323  
    324     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    325     *payload = host_to_usrp_u32(word0); 
    326  
    327     // The second word is solely the register value to be written 
    328     // FIXME: should this be unsigned? 
    329     payload += 1;  
    330     *payload = host_to_usrp_u32((uint32_t) val); 
    331      
    332     // Rebuild the header to update the payload length 
    333     int h_flags = flags(); 
    334     int h_chan = chan(); 
    335     int h_tag = tag(); 
    336     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_WRITEREG_LEN; 
    337  
    338     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    339  
    340     return true; 
    341   } 
    342    
    343   bool cs_write_reg_masked(long reg_num, long val, long mask) 
    344   { 
    345     if(!align32()) 
    346       return false; 
    347  
    348     int p_len = payload_len(); 
    349  
    350     if((MAX_PAYLOAD - p_len) < (CS_WRITEREGMASKED_LEN + CS_FIXED_LEN)) 
    351       return false; 
    352  
    353     uint32_t word0 = 0; 
    354  
    355     // Build the first word which includes the register number 
    356     word0 = ( 
    357         ((OP_WRITE_REG_MASKED & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    358       | ((CS_WRITEREGMASKED_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    359       | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT) 
    360     ); 
    361  
    362     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    363     *payload = host_to_usrp_u32(word0); 
    364  
    365     // Skip over the first word and write the register value 
    366     payload += 1; 
    367     *payload = host_to_usrp_u32((uint32_t) val); 
    368  
    369     // Skip over the register value and write the mask 
    370     payload += 1; 
    371     *payload = host_to_usrp_u32((uint32_t) mask); 
    372      
    373     // Rebuild the header to update the payload length 
    374     int h_flags = flags(); 
    375     int h_chan = chan(); 
    376     int h_tag = tag(); 
    377     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_WRITEREGMASKED_LEN; 
    378  
    379     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    380  
    381       return true; 
    382   } 
    383    
    384   bool cs_read_reg(long rid, long reg_num) 
    385   { 
    386     if(!align32()) 
    387       return false; 
    388  
    389     int p_len = payload_len(); 
    390  
    391     if((MAX_PAYLOAD - p_len) < (CS_READREG_LEN + CS_FIXED_LEN)) 
    392       return false; 
    393  
    394     uint32_t read_reg = (  
    395         ((OP_READ_REG & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    396       | ((CS_READREG_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    397       | ((rid & CS_RID_MASK) << CS_RID_SHIFT) 
    398       | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT) 
    399  
    400       ); 
    401  
    402     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    403     *payload = host_to_usrp_u32(read_reg); 
    404  
    405     // Update payload length 
    406     int h_flags = flags(); 
    407     int h_chan = chan(); 
    408     int h_tag = tag(); 
    409     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_READREG_LEN; 
    410  
    411     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    412  
    413     return true; 
    414   } 
    415    
    416   bool cs_read_reg_reply(long rid, long reg_num, long reg_val) 
    417   { 
    418     if(!align32()) 
    419       return false; 
    420  
    421     int p_len = payload_len(); 
    422  
    423     if((MAX_PAYLOAD - p_len) < (CS_READREGREPLY_LEN + CS_FIXED_LEN)) 
    424       return false; 
    425  
    426     uint32_t word0 = (  
    427         ((OP_READ_REG_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    428       | ((CS_READREGREPLY_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    429       | ((rid & CS_RID_MASK) << CS_RID_SHIFT) 
    430       | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT) 
    431  
    432       ); 
    433  
    434     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    435     *payload = host_to_usrp_u32(word0); 
    436  
    437     // Hop to the next word and write the reg value 
    438     payload += 1; 
    439     *payload = host_to_usrp_u32((uint32_t) reg_val);  
    440  
    441     // Update payload length 
    442     int h_flags = flags(); 
    443     int h_chan = chan(); 
    444     int h_tag = tag(); 
    445     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_READREGREPLY_LEN; 
    446  
    447     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    448  
    449     return true; 
    450   } 
    451  
    452   bool cs_delay(long ticks) 
    453   { 
    454     if(!align32()) 
    455       return false; 
    456  
    457     int p_len = payload_len(); 
    458  
    459     if((MAX_PAYLOAD - p_len) < (CS_DELAY_LEN + CS_FIXED_LEN)) 
    460       return false; 
    461  
    462     uint32_t delay = (  
    463         ((OP_DELAY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    464       | ((CS_DELAY_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    465       | ((ticks & CS_DELAY_MASK) << CS_DELAY_SHIFT) 
    466  
    467       ); 
    468  
    469     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    470     *payload = host_to_usrp_u32(delay); 
    471  
    472     // Update payload length 
    473     int h_flags = flags(); 
    474     int h_chan = chan(); 
    475     int h_tag = tag(); 
    476     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_DELAY_LEN; 
    477  
    478     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    479  
    480     return true; 
    481   } 
    482  
    483   bool cs_i2c_write(long i2c_addr, uint8_t *i2c_data, size_t data_len) 
    484   { 
    485     if(!align32()) 
    486       return false; 
    487  
    488     int p_len = payload_len(); 
    489  
    490     int i2c_len = data_len + 2;   // 2 bytes between mbz and addr 
    491  
    492     if((MAX_PAYLOAD - p_len) < (i2c_len + CS_FIXED_LEN)) 
    493       return false; 
    494  
    495     uint32_t word0 = 0; 
    496  
    497     word0 = ( 
    498         ((OP_I2C_WRITE & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    499       | ((i2c_len & CS_LEN_MASK) << CS_LEN_SHIFT) 
    500       | ((i2c_addr & CS_I2CADDR_MASK) << CS_I2CADDR_SHIFT) 
    501     ); 
    502  
    503     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    504      *payload = host_to_usrp_u32(word0); 
    505  
    506      // Jump over the first word and write the data 
    507      // FIXME: Should the data be changed to usrp byte order? 
    508      payload += 1; 
    509      memcpy(payload, i2c_data, data_len); 
    510  
    511     // Update payload length 
    512     int h_flags = flags(); 
    513     int h_chan = chan(); 
    514     int h_tag = tag(); 
    515     int h_payload_len = payload_len() + CS_FIXED_LEN + i2c_len; 
    516  
    517     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    518  
    519     return true; 
    520   } 
    521  
    522   bool cs_i2c_read(long rid, long i2c_addr, long n_bytes) 
    523   { 
    524     if(!align32()) 
    525       return false; 
    526  
    527     int p_len = payload_len(); 
    528  
    529     if((MAX_PAYLOAD - p_len) < (CS_I2CREAD_LEN + CS_FIXED_LEN)) 
    530       return false; 
    531  
    532     uint32_t word0 = 0; 
    533      
    534     word0 = (  
    535         ((OP_I2C_READ & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    536       | ((CS_I2CREAD_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    537       | ((rid & CS_RID_MASK) << CS_RID_SHIFT) 
    538       | ((i2c_addr & CS_I2CADDR_MASK) << CS_I2CADDR_SHIFT) 
    539       ); 
    540  
    541     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    542     *payload = host_to_usrp_u32(word0); 
    543  
    544     // Jump a word and write the number of bytes to read 
    545     payload += 1; 
    546     uint32_t word1 =  
    547       (n_bytes & CS_I2CREADBYTES_MASK) << CS_I2CREADBYTES_SHIFT; 
    548     *payload = host_to_usrp_u32(word1); 
    549  
    550     // Update payload length 
    551     int h_flags = flags(); 
    552     int h_chan = chan(); 
    553     int h_tag = tag(); 
    554     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_I2CREAD_LEN; 
    555  
    556     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    557  
    558     return true; 
    559   } 
    560    
    561   bool cs_i2c_read_reply(long rid, long i2c_addr, uint8_t *i2c_data, long i2c_data_len) 
    562   { 
    563     if(!align32()) 
    564       return false; 
    565  
    566     int p_len = payload_len(); 
    567  
    568     int i2c_len = i2c_data_len + 2; 
    569  
    570     if((MAX_PAYLOAD - p_len) < (i2c_len + CS_FIXED_LEN))  
    571       return false; 
    572      
    573     uint32_t word0 = 0; 
    574      
    575     word0 = (  
    576         ((OP_I2C_READ_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    577       | ((i2c_len & CS_LEN_MASK) << CS_LEN_SHIFT) 
    578       | ((rid & CS_RID_MASK) << CS_RID_SHIFT) 
    579       | ((i2c_addr & CS_I2CADDR_MASK) << CS_I2CADDR_SHIFT) 
    580       ); 
    581  
    582     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    583     *payload = host_to_usrp_u32(word0); 
    584  
    585     // Jump a word and write the actual data 
    586     payload += 1; 
    587     memcpy(payload, i2c_data, i2c_data_len); 
    588  
    589     // Update payload length 
    590     int h_flags = flags(); 
    591     int h_chan = chan(); 
    592     int h_tag = tag(); 
    593     int h_payload_len = payload_len() + CS_FIXED_LEN + i2c_len; 
    594  
    595     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    596  
    597     return true; 
    598   } 
    599  
    600   bool cs_spi_write(long enables, long format, long opt_header_bytes, uint8_t *spi_data, long spi_data_len) 
    601   { 
    602     if(!align32()) 
    603       return false; 
    604  
    605     int p_len = payload_len(); 
    606  
    607     int spi_len = spi_data_len + 6; 
    608  
    609     if((MAX_PAYLOAD - p_len) < (spi_len + CS_FIXED_LEN)) 
    610       return false; 
    611  
    612     uint32_t word = 0; 
    613  
    614     // First word contains the opcode and length, then mbz 
    615     word = ( 
    616         ((OP_SPI_WRITE & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    617       | ((spi_len & CS_LEN_MASK) << CS_LEN_SHIFT) 
    618       ); 
    619     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    620     *payload = host_to_usrp_u32(word); 
    621  
    622     payload += 1; 
    623  
    624     // Second word contains the enables, format, and optional tx bytes 
    625     word = 0; 
    626     word = ( 
    627         ((enables & CS_SPIENABLES_MASK) << CS_SPIENABLES_SHIFT) 
    628       | ((format & CS_SPIFORMAT_MASK) << CS_SPIFORMAT_SHIFT) 
    629       | ((opt_header_bytes & CS_SPIOPT_MASK) << CS_SPIOPT_SHIFT) 
    630       ); 
    631     payload = (uint32_t *) (d_payload + p_len); 
    632     *payload = host_to_usrp_u32(word); 
    633  
    634     payload += 1; 
    635     memcpy(payload, spi_data, spi_data_len); 
    636  
    637     // Update payload length 
    638     int h_flags = flags(); 
    639     int h_chan = chan(); 
    640     int h_tag = tag(); 
    641     int h_payload_len = payload_len() + CS_FIXED_LEN + spi_len; 
    642  
    643     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    644  
    645     return true; 
    646   } 
    647  
    648   bool cs_spi_read(long rid, long enables, long format, long opt_header_bytes, long n_bytes) 
    649   { 
    650     if(!align32()) 
    651       return false; 
    652  
    653     int p_len = payload_len(); 
    654  
    655     if((MAX_PAYLOAD - p_len) < (CS_SPIREAD_LEN + CS_FIXED_LEN)) 
    656       return false; 
    657  
    658     uint32_t word = 0; 
    659  
    660     // First word contains the opcode, length, and RID 
    661     word = ( 
    662         ((OP_SPI_READ & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    663       | ((CS_SPIREAD_LEN & CS_LEN_MASK) << CS_LEN_SHIFT) 
    664       | ((rid & CS_RID_MASK) << CS_RID_SHIFT) 
    665       ); 
    666     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    667     *payload = host_to_usrp_u32(word); 
    668  
    669     payload += 1; 
    670  
    671     // Second word contains the enables, format, and optional tx bytes 
    672     word = 0; 
    673     word = ( 
    674         ((enables & CS_SPIENABLES_MASK) << CS_SPIENABLES_SHIFT) 
    675       | ((format & CS_SPIFORMAT_MASK) << CS_SPIFORMAT_SHIFT) 
    676       | ((opt_header_bytes & CS_SPIOPT_MASK) << CS_SPIOPT_SHIFT) 
    677       ); 
    678     payload = (uint32_t *) (d_payload + p_len); 
    679     *payload = host_to_usrp_u32(word); 
    680  
    681     payload += 1; 
    682  
    683     // The third word contains the number of bytes 
    684     word = 0; 
    685     word = ( 
    686         ((n_bytes & CS_SPINBYTES_MASK) << CS_SPINBYTES_SHIFT) 
    687       ); 
    688     payload = (uint32_t *) (d_payload + p_len); 
    689     *payload = host_to_usrp_u32(word); 
    690  
    691     // Update payload length 
    692     int h_flags = flags(); 
    693     int h_chan = chan(); 
    694     int h_tag = tag(); 
    695     int h_payload_len = payload_len() + CS_FIXED_LEN + CS_SPIREAD_LEN; 
    696  
    697     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    698      
    699     return true; 
    700   } 
    701  
    702   bool cs_spi_read_reply(long rid, uint8_t *spi_data, long spi_data_len) 
    703   { 
    704     if(!align32()) 
    705       return false; 
    706  
    707     int p_len = payload_len(); 
    708  
    709     int spi_len = spi_data_len + 2; 
    710  
    711     if((MAX_PAYLOAD - p_len) < (spi_len + CS_FIXED_LEN)) 
    712       return false; 
    713  
    714     uint32_t word = 0; 
    715  
    716     // First word contains the opcode, length, and RID 
    717     word = ( 
    718         ((OP_SPI_READ_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT) 
    719       | ((spi_len & CS_LEN_MASK) << CS_LEN_SHIFT) 
    720       | ((rid & CS_RID_MASK) << CS_RID_SHIFT) 
    721       ); 
    722     uint32_t *payload = (uint32_t *) (d_payload + p_len); 
    723     *payload = host_to_usrp_u32(word); 
    724  
    725     // Jump a word and write the actual data 
    726     payload += 1; 
    727     memcpy(payload, spi_data, spi_data_len); 
    728  
    729     // Update payload length 
    730     int h_flags = flags(); 
    731     int h_chan = chan(); 
    732     int h_tag = tag(); 
    733     int h_payload_len = payload_len() + CS_FIXED_LEN + spi_len; 
    734  
    735     set_header(h_flags, h_chan, h_tag, h_payload_len); 
    736  
    737     return true; 
    738   } 
    739  
    740   // The following method takes an offset within the packet payload to extract 
    741   // a control/status subpacket and construct a pmt response which includes the 
    742   // proper signal and arguments specified by usrp-low-level-cs.  The USRP 
    743   // server could therefore use this to read subpackets and pass them responses 
    744   // back up to the application.  It's arguable that only reply packets should 
    745   // be parsed here, however we parse others for use in debugging or failure 
    746   // reporting on the transmit side of packets. 
    747   pmt_t read_subpacket(int payload_offset) { 
    748  
    749     uint32_t subpkt = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset))); 
    750     uint32_t opcode = (subpkt >> CS_OPCODE_SHIFT) & CS_OPCODE_MASK; 
    751     uint32_t len = (subpkt >> CS_LEN_SHIFT) & CS_LEN_MASK; 
    752  
    753     switch(opcode) { 
    754        
    755       case OP_PING_FIXED_REPLY: 
    756       { 
    757         pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK); 
    758         pmt_t pingval = pmt_from_long((subpkt >> CS_PINGVAL_SHIFT) & CS_PINGVAL_MASK); 
    759         return pmt_list3(s_op_ping_fixed_reply, rid, pingval); 
    760       } 
    761  
    762       case OP_READ_REG_REPLY: 
    763       { 
    764         pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK); 
    765         pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & CS_REGNUM_MASK); 
    766  
    767         // To get the register value we just read the next 32 bits 
    768         uint32_t val  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4))); 
    769         pmt_t reg_val = pmt_from_long(val); 
    770  
    771         return pmt_list4(s_op_read_reg_reply, rid, reg_num, reg_val); 
    772       } 
    773  
    774       case OP_I2C_READ_REPLY: 
    775       { 
    776         pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK); 
    777         pmt_t i2c_addr  = pmt_from_long((subpkt >> CS_I2CADDR_SHIFT) & CS_I2CADDR_MASK); 
    778  
    779         // Make a u8 vector to dump the data from the packet into 
    780         size_t i2c_data_len; 
    781         pmt_t i2c_data  = pmt_make_u8vector(len - 2, 0);   // skip rid+mbz+addr = 2 bytes 
    782         uint8_t *w_data  =  
    783             (uint8_t *) pmt_u8vector_writeable_elements(i2c_data, i2c_data_len); 
    784  
    785         memcpy(w_data, d_payload + payload_offset + 4, i2c_data_len);  // skip first word 
    786  
    787         return pmt_list4(s_op_i2c_read_reply, rid, i2c_addr, i2c_data); 
    788       } 
    789  
    790       case OP_SPI_READ_REPLY: 
    791       { 
    792         pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK); 
    793          
    794         // Make a u8 vector to dump the data from the packet into 
    795         size_t spi_data_len; 
    796         pmt_t spi_data  = pmt_make_u8vector(len - 2, 0);   // skip rid+mbz+addr = 2 bytes 
    797         uint8_t *w_data  =  
    798             (uint8_t *) pmt_u8vector_writeable_elements(spi_data, spi_data_len); 
    799  
    800         memcpy(w_data, d_payload + payload_offset + 4, spi_data_len);  // skip first word 
    801  
    802         return pmt_list3(s_op_spi_read_reply, rid, spi_data); 
    803       } 
    804  
    805       case OP_PING_FIXED: 
    806       { 
    807         pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK); 
    808         pmt_t pingval = pmt_from_long((subpkt >> CS_PINGVAL_SHIFT) & CS_PINGVAL_MASK); 
    809         return pmt_list3(s_op_ping_fixed, rid, pingval); 
    810       } 
    811  
    812       case OP_WRITE_REG: 
    813       { 
    814         pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & CS_REGNUM_MASK); 
    815  
    816         // To get the register value we just read the next 32 bits 
    817         uint32_t val  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4))); 
    818         pmt_t reg_val = pmt_from_long(val); 
    819  
    820         return pmt_list3(s_op_write_reg, reg_num, reg_val); 
    821       } 
    822  
    823       case OP_WRITE_REG_MASKED: 
    824       { 
    825         pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & CS_REGNUM_MASK); 
    826  
    827         // To get the register value we just read the next 32 bits 
    828         uint32_t val  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4))); 
    829         pmt_t reg_val = pmt_from_long(val); 
    830  
    831         // The mask is the next 32 bits 
    832         uint32_t mask  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 8))); 
    833         pmt_t reg_mask = pmt_from_long(mask); 
    834  
    835         return pmt_list4(s_op_write_reg_masked, reg_num, reg_val, reg_mask); 
    836       } 
    837  
    838       case OP_READ_REG: 
    839       { 
    840         pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK); 
    841         pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & CS_REGNUM_MASK); 
    842  
    843         return pmt_list3(s_op_read_reg, rid, reg_num); 
    844       } 
    845  
    846       case OP_I2C_WRITE: 
    847       { 
    848         pmt_t i2c_addr    = pmt_from_long((subpkt >> CS_I2CADDR_SHIFT) & CS_I2CADDR_MASK); 
    849  
    850         // The length includes an extra 2 bytes for storing the mbz and addr 
    851         pmt_t i2c_data    = pmt_make_u8vector(len-2, 0); 
    852  
    853         // Get a writeable address to copy the data from the packet 
    854         size_t ignore; 
    855         uint8_t *w_data = (uint8_t *) pmt_u8vector_writeable_elements(i2c_data, ignore); 
    856         memcpy(w_data, d_payload + payload_offset + 4, len-2); 
    857  
    858          
    859         return pmt_list3(s_op_i2c_write, i2c_addr, i2c_data); 
    860       } 
    861  
    862       case OP_I2C_READ: 
    863       { 
    864         pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK); 
    865         pmt_t i2c_addr  = pmt_from_long((subpkt >> CS_I2CADDR_SHIFT) & CS_I2CADDR_MASK); 
    866          
    867         // The number of bytes is in the next word 
    868         uint32_t bytes  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4))); 
    869         bytes = (bytes >> CS_I2CREADBYTES_SHIFT) & CS_I2CREADBYTES_MASK; 
    870         pmt_t i2c_bytes = pmt_from_long(bytes); 
    871  
    872         return pmt_list4(s_op_i2c_read, rid, i2c_addr, i2c_bytes); 
    873       } 
    874  
    875       case OP_SPI_WRITE: 
    876       { 
    877         // Nothing interesting in the first word, skip to the next 
    878         uint32_t word  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4))); 
    879         pmt_t enables   = pmt_from_long((word >> CS_SPIENABLES_SHIFT) & CS_SPIENABLES_MASK); 
    880         pmt_t format    = pmt_from_long((word >> CS_SPIFORMAT_SHIFT) & CS_SPIFORMAT_MASK); 
    881         pmt_t opt       = pmt_from_long((word >> CS_SPIOPT_SHIFT) & CS_SPIOPT_MASK); 
    882  
    883         // From the next word and on is data 
    884         size_t spi_data_len; 
    885         pmt_t spi_data  = pmt_make_u8vector(len - 6, 0);   // skip rid+mbz+addr = 2 bytes 
    886         uint8_t *w_data  =  
    887             (uint8_t *) pmt_u8vector_writeable_elements(spi_data, spi_data_len); 
    888  
    889         memcpy(w_data, d_payload + payload_offset + 8, spi_data_len);  // skip first 2 words 
    890  
    891         return pmt_list5(s_op_spi_write, enables, format, opt, spi_data); 
    892       } 
    893  
    894       case OP_SPI_READ: 
    895       { 
    896         // Read the RID from the first word, the rest is mbz 
    897         pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK); 
    898  
    899         // Continue at the next word... 
    900         uint32_t word  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 4))); 
    901         pmt_t enables   = pmt_from_long((word >> CS_SPIENABLES_SHIFT) & CS_SPIENABLES_MASK); 
    902         pmt_t format    = pmt_from_long((word >> CS_SPIFORMAT_SHIFT) & CS_SPIFORMAT_MASK); 
    903         pmt_t opt       = pmt_from_long((word >> CS_SPIOPT_SHIFT) & CS_SPIOPT_MASK); 
    904  
    905         // The number of bytes is the only thing to read in the next word 
    906         word  = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset + 8))); 
    907         pmt_t n_bytes   = pmt_from_long((word >> CS_SPINBYTES_SHIFT) & CS_SPINBYTES_MASK); 
    908  
    909         return pmt_list6(s_op_spi_read, rid, enables, format, opt, n_bytes); 
    910       } 
    911  
    912       case OP_DELAY: 
    913       { 
    914         pmt_t ticks = pmt_from_long((subpkt >> CS_DELAY_SHIFT) & CS_DELAY_MASK); 
    915  
    916         return pmt_list2(s_op_delay, ticks); 
    917       } 
    918        
    919       default: 
    920         return PMT_NIL; 
    921  
    922     } 
    923   } 
    924  
    925   // Takes an offset to the beginning of a subpacket and extracts the 
    926   // length of the subpacket 
    927   int cs_len(int payload_offset) { 
    928     uint32_t subpkt = usrp_to_host_u32(*((uint32_t *)(d_payload + payload_offset))); 
    929     return (subpkt >> CS_LEN_SHIFT) & CS_LEN_MASK; 
    930   } 
    931  
     215  // C/S methods 
     216  bool align32(); 
     217  bool cs_ping(long rid, long ping_val); 
     218  bool cs_ping_reply(long rid, long ping_val); 
     219  bool cs_write_reg(long reg_num, long val); 
     220  bool cs_write_reg_masked(long reg_num, long val, long mask); 
     221  bool cs_read_reg(long rid, long reg_num); 
     222  bool cs_read_reg_reply(long rid, long reg_num, long reg_val); 
     223  bool cs_delay(long ticks); 
     224  bool cs_i2c_write(long i2c_addr, uint8_t *i2c_data, size_t data_len); 
     225  bool cs_i2c_read(long rid, long i2c_addr, long n_bytes); 
     226  bool cs_i2c_read_reply(long rid, long i2c_addr, uint8_t *i2c_data, long i2c_data_len); 
     227  bool cs_spi_write(long enables, long format, long opt_header_bytes, uint8_t *spi_data, long spi_data_len); 
     228  bool cs_spi_read(long rid, long enables, long format, long opt_header_bytes, long n_bytes); 
     229  bool cs_spi_read_reply(long rid, uint8_t *spi_data, long spi_data_len); 
     230  int cs_len(int payload_offset); 
     231  pmt_t read_subpacket(int payload_offset); 
    932232}; 
    933233