// FlashPage writes 1 page to Crazyflie flash storage
func FlashPage(dev cflie.Device, info Info, page int, mem []byte) (err error) {
if len(mem) != info.PageSize {
return fmt.Errorf("FlashPage: %d = len(mem) != info.PageSize = %d", len(mem), info.PageSize)
}
if page < info.FlashStart {
return fmt.Errorf("FlashPage: %d = page < FlashStart = %d", page, info.FlashStart)
}
if page >= info.FlashPages {
return fmt.Errorf("FlashPage: %d = page >= info.FlashPages = %d", page, info.FlashPages)
}
buf := make([]byte, 128)
got := make(map[int]bool)
// 1. Load page to memory buffer and verify that all the data is correct
for try := 0; try < 10; try++ {
// Load buffer
for offset := 0; offset < info.PageSize; offset += 16 {
if got[offset] {
// Skip chunks which are already in the buffer
continue
}
_, err = dev.Write(loadBuffer(0, uint16(offset), mem[offset:offset+16]))
if err != nil {
log.Printf("write: %v", err)
}
}
// Read buffer
for rtry := 0; rtry <= 2; rtry++ {
for offset := 0; offset < info.PageSize; offset += 16 {
if got[offset] {
continue
}
_, err = dev.Write(readBuffer(0, uint16(offset)))
if err != nil {
log.Printf("write: %v", err)
continue
}
n, err := dev.Read(buf)
if err != nil {
log.Printf("read: n: %d, err: %v", n, err)
continue
}
if n == 0 {
log.Printf("Empty packet")
continue
}
// First byte is auxiliary
p := buf[1:n]
if len(p) < 10 || p[2] != CMD_READ_BUFFER {
// Some weird packet; ignore it
continue
}
inPage := int(p[3]) + (int(p[4]) << 8)
if inPage != 0 {
log.Printf("%d = inPage != 0", inPage)
continue
}
inOffset := int(p[5]) + (int(p[6]) << 8)
inData := p[7 : 7+16]
// Check that the contents are correct
ok := true
for i, v := range inData {
if mem[inOffset+i] != v {
log.Printf("Chunk with incorrect data detected, offset=%d",
inOffset)
ok = false
break
}
}
if ok {
got[inOffset] = true
}
}
}
}
// Check that we got all chunks to the buffer
ok := true
for offset := 0; offset < info.PageSize; offset += 16 {
if !got[offset] {
log.Printf("Failed to write a chunk into a buffer, offset=%d", offset)
ok = false
}
}
if !ok {
return fmt.Errorf("Some chunks failed to be loaded into Crazyflie memory buffer")
}
log.Printf("Data for page #%d loaded into Crazyflie memory buffer", page)
// 2. Write from memory buffer to Flash
for try := 0; try < 10; try++ {
_, err := dev.Write(writeFlash(0, uint16(page), 1))
if err != nil {
log.Printf("Unable to send CMD_WRITE_FLASH packet: %v", err)
}
deadline := time.Now().Add(time.Second)
ok := false
for time.Now().Before(deadline) {
dev.Write([]byte{0xFF})
n, err := dev.Read(buf)
if err != nil {
log.Printf("read: %v", err)
continue
}
// First byte is auxiliary
p := buf[1:n]
if len(p) < 4 || p[2] != CMD_WRITE_FLASH {
// Some weird packet; ignore it
continue
}
if p[3] != 1 /* done */ || p[4] != 0 /* error */ {
log.Printf("Flashing attempt failed, done: %d, error: %d", p[3], p[4])
continue
}
ok = true
break
}
if ok {
break
}
}
log.Printf("Page %d seems to be written, verifying...", page)
// 3. Read Flash page and verify
dump, err := Dump(dev, info, page, page+1)
if err != nil {
return fmt.Errorf("Failed to dump the contents of page #%d: %v", page, err)
}
if !bytes.Equal(mem, dump) {
return fmt.Errorf("Page #%d has unexpected contents", page)
}
return nil
}
// Dump downloads a region of Flash memory from Crazyflie. Device must be already connected to the bootloader.
func Dump(dev cflie.Device, info Info, fromPage, toPage int) (mem []byte, err error) {
buf := make([]byte, 128)
got := make(map[int]bool)
mem = make([]byte, (toPage-fromPage)*info.PageSize)
for try := 0; try < 10; try++ {
for page := fromPage; page < toPage; page++ {
if try == 0 {
fmt.Fprintf(os.Stderr, ".")
}
for offset := 0; offset < info.PageSize; offset += 16 {
start := page*info.PageSize + offset
if got[start] {
// Do not request already received chunks
continue
}
if try > 0 {
fmt.Fprintf(os.Stderr, "{Retry: %d}", start)
}
_, err = dev.Write(readFlash(uint16(page), uint16(offset)))
if err != nil {
log.Printf("write: %v", err)
continue
}
n, err := dev.Read(buf)
if err != nil {
log.Printf("read: n: %d, err: %v", n, err)
continue
}
if n == 0 {
log.Printf("Empty packet")
continue
}
p := buf[1:n]
if len(p) > 10 && p[2] == CMD_READ_FLASH {
page := int(p[3]) + (int(p[4]) << 8)
offset := int(p[5]) + (int(p[6]) << 8)
data := p[7 : 7+16]
start := page*info.PageSize + offset
got[start] = true
index := start - fromPage*info.PageSize
copy(mem[index:index+16], data)
}
}
}
}
missing := false
for page := fromPage; page < toPage; page++ {
for offset := 0; offset < info.PageSize; offset += 16 {
start := page*info.PageSize + offset
if !got[start] {
log.Printf("Missing chunk: index=%d", start)
missing = true
}
}
}
fmt.Fprintf(os.Stderr, "\n")
if missing {
return nil, fmt.Errorf("Some chunks are failed to download")
}
return
}
