Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_RAMTRON_ENABLED
       4             : 
       5             : #include "SIM_RAMTRON.h"
       6             : 
       7             : #include <errno.h>
       8             : #include <unistd.h>
       9             : #include <sys/types.h>
      10             : #include <sys/stat.h>
      11             : #include <fcntl.h>
      12             : 
      13             : #include <AP_HAL_SITL/AP_HAL_SITL.h>
      14             : 
      15             : using namespace SITL;
      16             : 
      17             : extern const HAL_SITL& hal_sitl;
      18             : 
      19           3 : void RAMTRON::open_storage_fd()
      20             : {
      21           3 :     if (storage_fd != -1) {
      22           0 :         AP_HAL::panic("Should not have been called");
      23             :     }
      24           3 :     const char *filepath = filename();
      25           3 :     uint32_t flags = O_RDWR|O_CREAT;
      26           3 :     if (hal_sitl.get_wipe_storage()) {
      27           0 :         flags |= O_TRUNC;
      28             :     }
      29           3 :     storage_fd = open(filepath, flags, 0644);
      30           3 :     if (storage_fd == -1) {
      31           0 :         AP_HAL::panic("open(%s): %s", filepath, strerror(errno));
      32             :     }
      33           3 :     if (ftruncate(storage_fd, storage_size()) != 0) {
      34           0 :         AP_HAL::panic("truncate(%s): %s", filepath, strerror(errno));
      35             :     }
      36           3 : }
      37             : 
      38        1574 : int RAMTRON::rdwr(uint8_t count, SPI::spi_ioc_transfer *&tfrs)
      39             : {
      40        1574 :     if (storage_fd == -1) {
      41           3 :         open_storage_fd();
      42             :     }
      43             : 
      44             :     // commands:
      45             :     static const uint8_t RAMTRON_RDID  = 0x9f;
      46             :     static const uint8_t RAMTRON_READ  = 0x03;
      47             :     static const uint8_t RAMTRON_WREN  = 0x06;
      48             :     static const uint8_t RAMTRON_WRITE = 0x02;
      49             : 
      50        3151 :     for (uint8_t i=0; i<count; i++) {
      51        1577 :         SPI::spi_ioc_transfer &tfr = tfrs[i];
      52        1577 :         uint8_t *tx_buf = (uint8_t*)(tfr.tx_buf);
      53        1577 :         uint8_t *rx_buf = (uint8_t*)(tfr.rx_buf);
      54             : 
      55        1577 :         switch (state) {
      56         792 :         case State::WAITING: {
      57             :             // find a command
      58         792 :             uint8_t command = tx_buf[0];
      59         792 :             switch (command) {
      60           3 :             case RAMTRON_RDID:
      61           3 :                 state = State::READING_RDID;
      62           3 :                 break;
      63         775 :             case RAMTRON_READ:
      64         775 :                 xfr_addr = tx_buf[1] << 8 | tx_buf[2];
      65         775 :                 state = State::READING;
      66         775 :                 break;
      67           7 :             case RAMTRON_WRITE:
      68           7 :                 xfr_addr = tx_buf[1] << 8 | tx_buf[2];
      69           7 :                 state = State::WRITING;
      70           7 :                 break;
      71           7 :             case RAMTRON_WREN:
      72           7 :                 write_enabled = true;
      73           7 :                 break;
      74           0 :             default:
      75           0 :                 abort();
      76             :             }
      77         792 :             break;
      78             :         }
      79           3 :         case State::READING_RDID:
      80           3 :             fill_rdid(rx_buf, tfr.len);
      81           3 :             state = State::WAITING;
      82           3 :             break;
      83         775 :         case State::READING: {
      84         775 :             if (xfr_addr + tfr.len > storage_size()) {
      85           0 :                 abort();
      86             :             }
      87         775 :             if (lseek(storage_fd, xfr_addr, SEEK_SET) == -1) {
      88           0 :                 AP_HAL::panic("lseek(): %s", strerror(errno));
      89             :             }
      90         775 :             const size_t read_ret = read(storage_fd, rx_buf, tfr.len);
      91         775 :             if (read_ret != tfr.len) {
      92           0 :                 AP_HAL::panic("read(): %s (%d/%u)", strerror(errno), (signed)read_ret, (unsigned)tfr.len);
      93             :             }
      94         775 :             state = State::WAITING;
      95         775 :             break;
      96             :         }
      97           7 :         case State::WRITING: {
      98           7 :             if (!write_enabled) {
      99           0 :                 AP_HAL::panic("Writes not enabled");
     100             :             }
     101           7 :             if (xfr_addr + tfr.len > storage_size()) {
     102           0 :                 abort();
     103             :             }
     104           7 :             if (lseek(storage_fd, xfr_addr, SEEK_SET) == -1) {
     105           0 :                 AP_HAL::panic("lseek(): %s", strerror(errno));
     106             :             }
     107           7 :             const size_t write_ret = write(storage_fd, tx_buf, tfr.len);
     108           7 :             if (write_ret != tfr.len) {
     109           0 :                 AP_HAL::panic("write(): %s (%d/%u)", strerror(errno), (signed)write_ret, (unsigned)tfr.len);
     110             :             }
     111           7 :             state = State::WAITING;
     112           7 :             write_enabled = false;
     113           7 :             break;
     114             :         }
     115             :         }
     116             :     }
     117        1574 :     return 0;
     118             : }
     119             : 
     120             : #endif  // AP_SIM_RAMTRON_ENABLED