Line data Source code
1 : #include "SIM_JEDEC.h" 2 : 3 : #if AP_SIM_JEDEC_ENABLED 4 : 5 : #include <errno.h> 6 : #include <unistd.h> 7 : #include <sys/types.h> 8 : #include <sys/stat.h> 9 : #include <fcntl.h> 10 : 11 : #include <AP_HAL_SITL/AP_HAL_SITL.h> 12 : 13 : using namespace SITL; 14 : 15 : extern const HAL_SITL& hal_sitl; 16 : 17 5 : void JEDEC::open_storage_fd() 18 : { 19 5 : if (storage_fd != -1) { 20 0 : AP_HAL::panic("Should not have been called"); 21 : } 22 5 : const char *filepath = filename(); 23 5 : if (hal_sitl.get_wipe_storage()) { 24 0 : unlink(filepath); 25 : } 26 5 : storage_fd = open(filepath, O_RDWR|O_CREAT, 0644); 27 5 : if (storage_fd == -1) { 28 0 : AP_HAL::panic("open(%s): %s", filepath, strerror(errno)); 29 : } 30 5 : if (ftruncate(storage_fd, get_storage_size()) != 0) { 31 0 : AP_HAL::panic("truncate(%s): %s", filepath, strerror(errno)); 32 : } 33 5 : } 34 : 35 9879 : void JEDEC::sector4k_erase (uint32_t addr) 36 : { 37 167943 : for (uint8_t i=0; i<get_page_per_sector(); i++) { 38 158064 : page_erase(addr + i*get_page_size()); 39 : } 40 9879 : } 41 : 42 617 : void JEDEC::block64k_erase (uint32_t addr) 43 : { 44 : // we have 16 sectors in a block 45 10489 : for (uint16_t i=0; i<16; i++) { 46 9872 : sector4k_erase(addr + i*get_page_per_sector()*get_page_size()); 47 : } 48 617 : } 49 : 50 158064 : void JEDEC::page_erase (uint32_t addr) 51 : { 52 316128 : const uint32_t fill_length = get_page_size(); 53 158064 : uint8_t fill[fill_length]; 54 158064 : if (addr + fill_length > get_storage_size()) { 55 0 : AP_HAL::panic("trying to write outside memory"); 56 : } 57 158064 : memset(fill, 0xFF, sizeof(fill)); 58 158064 : const size_t write_ret = pwrite(storage_fd, fill, sizeof(fill), addr); 59 158064 : if (write_ret != sizeof(fill)) { 60 0 : printf("Failed page erase"); 61 : } 62 158064 : } 63 : 64 7 : void JEDEC::bulk_erase() 65 : { 66 455 : for (uint16_t i=0; i<get_num_blocks(); i++) { 67 448 : block64k_erase(i*get_page_per_block()*get_page_size()); 68 : } 69 7 : } 70 : 71 72986 : uint32_t JEDEC::parse_addr (uint8_t* buffer, uint32_t len) 72 : { 73 72986 : if (len<4) { 74 0 : AP_HAL::panic("address too short"); 75 : } 76 : 77 : // buffer[0] is cmd 78 72986 : return buffer[1] << 16 | buffer[2] << 8 | buffer[3]; 79 : } 80 : 81 42708 : void JEDEC::assert_writes_enabled() 82 : { 83 42708 : if (!write_enabled) { 84 0 : AP_HAL::panic("Writes not enabled"); 85 : } 86 42708 : } 87 : 88 261521 : int JEDEC::rdwr(uint8_t count, SPI::spi_ioc_transfer *&tfrs) 89 : { 90 261521 : if (storage_fd == -1) { 91 5 : open_storage_fd(); 92 : } 93 : 94 : // commands: 95 : static const uint8_t JEDEC_RDID = 0x9f; 96 : static const uint8_t JEDEC_READ = 0x03; 97 : static const uint8_t JEDEC_WREN = 0x06; 98 : static const uint8_t JEDEC_WRITE = 0x02; 99 : static const uint8_t JEDEC_RDSR = 0x05; 100 : static const uint8_t JEDEC_SECTOR4_ERASE = 0x20; 101 : static const uint8_t JEDEC_BULK_ERASE = 0xC7; 102 : static const uint8_t JEDEC_BLOCK64_ERASE = 0xD8; 103 : 104 596052 : for (uint8_t i=0; i<count; i++) { 105 334531 : SPI::spi_ioc_transfer &tfr = tfrs[i]; 106 334531 : uint8_t *tx_buf = (uint8_t*)(tfr.tx_buf); 107 334531 : uint8_t *rx_buf = (uint8_t*)(tfr.rx_buf); 108 : 109 334531 : switch (state) { 110 188711 : case State::WAITING: { 111 : // find a command 112 188711 : uint8_t command = tx_buf[0]; 113 188711 : switch (command) { 114 5 : case JEDEC_RDID: 115 5 : state = State::READING_RDID; 116 5 : break; 117 30285 : case JEDEC_READ: 118 30285 : xfr_addr = parse_addr(tx_buf, tfr.len); 119 30285 : state = State::READING; 120 30285 : break; 121 42525 : case JEDEC_WRITE: 122 42525 : xfr_addr = parse_addr(tx_buf, tfr.len); 123 42525 : state = State::WRITING; 124 42525 : break; 125 42708 : case JEDEC_WREN: 126 42708 : write_enabled = true; 127 42708 : break; 128 73005 : case JEDEC_RDSR: 129 73005 : state = State::READING_RDSR; 130 73005 : break; 131 7 : case JEDEC_SECTOR4_ERASE: { 132 7 : xfr_addr = parse_addr(tx_buf, tfr.len); 133 7 : assert_writes_enabled(); 134 7 : sector4k_erase(xfr_addr); 135 7 : write_enabled = false; 136 7 : break; 137 : } 138 7 : case JEDEC_BULK_ERASE: { 139 7 : assert_writes_enabled(); 140 7 : bulk_erase(); 141 7 : write_enabled = false; 142 7 : break; 143 : } 144 169 : case JEDEC_BLOCK64_ERASE: { 145 169 : xfr_addr = parse_addr(tx_buf, tfr.len); 146 169 : assert_writes_enabled(); 147 169 : block64k_erase(xfr_addr); 148 169 : write_enabled = false; 149 169 : break; 150 : } 151 0 : default: 152 0 : AP_HAL::panic("Unhandled command received"); 153 : } 154 188711 : break; 155 : } 156 5 : case State::READING_RDID: 157 5 : fill_rdid(rx_buf, tfr.len); 158 5 : state = State::WAITING; 159 5 : break; 160 73005 : case State::READING_RDSR: 161 73005 : fill_rdsr(rx_buf, tfr.len); 162 73005 : state = State::WAITING; 163 73005 : break; 164 30285 : case State::READING: { 165 30285 : if (xfr_addr + tfr.len > get_storage_size()) { 166 0 : AP_HAL::panic("trying to read outside memory"); 167 : } 168 30285 : if (lseek(storage_fd, xfr_addr, SEEK_SET) == -1) { 169 0 : AP_HAL::panic("lseek(): %s", strerror(errno)); 170 : } 171 30285 : const size_t read_ret = read(storage_fd, rx_buf, tfr.len); 172 30285 : if (read_ret != tfr.len) { 173 0 : AP_HAL::panic("read(): %s (%d/%u)", strerror(errno), (signed)read_ret, (unsigned)tfr.len); 174 : } 175 30285 : state = State::WAITING; 176 30285 : break; 177 : } 178 42525 : case State::WRITING: { 179 42525 : assert_writes_enabled(); 180 42525 : if (xfr_addr + tfr.len > get_storage_size()) { 181 0 : AP_HAL::panic("trying to write outside memory"); 182 : } 183 42525 : if (lseek(storage_fd, xfr_addr, SEEK_SET) == -1) { 184 0 : AP_HAL::panic("lseek(): %s", strerror(errno)); 185 : } 186 42525 : const size_t write_ret = write(storage_fd, tx_buf, tfr.len); 187 42525 : if (write_ret != tfr.len) { 188 0 : AP_HAL::panic("write(): %s (%d/%u)", strerror(errno), (signed)write_ret, (unsigned)tfr.len); 189 : } 190 42525 : state = State::WAITING; 191 42525 : write_enabled = false; 192 42525 : break; 193 : } 194 : } 195 : } 196 261521 : return 0; 197 : } 198 : 199 : #endif // AP_SIM_JEDEC_ENABLED
