debug bridge FPGA memif access unaligned read/writes

[soybean-hust]

bool retval = true;
uint32_t rdata;
uint32_t wdata;
uint8_t * ptr;
uint8_t be;
unsigned int t_addr;

if (write) {
// write

/* BE is  not implemented  in SPI cmds*/
/* So we readback ~Align(4) data to writeback*/

/* only write 1B for address 1.3*/
ptr = (uint8_t *) &wdata;
if (addr&0x03)
    this->mem_read(addr&(~0x03), &wdata);
    
while(t_addr=(addr&0x03)) {
    
    printf("[e]addr =%x , v=%x", addr, *buffer);
    
    if (t_addr!=0x02){
        /* 1 or 3*/
     be = 1 << t_addr;   
     ptr[t_addr] = *((uint8_t *)buffer); 
     printf("odd write be=%x", be);
     retval = retval && this->mem_write(addr, be, wdata);
     addr   += 1;
     size   -= 1;
     buffer += 1;
    }
    if (t_addr==2)
    {
      if (size==1){
        be=0x04;
        ptr[2] = *((uint8_t *)buffer);
        retval = retval && this->mem_write(addr, be, wdata);
        size -= 1;
        addr += 1;
        buffer += 1;
      }
      else if(size>1){
        be=12;
        size -= 2;
        ptr[2] = *((uint8_t *)buffer);
        buffer++;
        ptr[3] = *((uint8_t *)buffer);
        retval = retval && this->mem_write(addr, be, wdata);
        addr += 2;
        buffer ++;            
      }
      else break;
    }
}
         
    

while (size >= 4) {
  retval = retval && this->mem_write(addr, 0xF, *((uint32_t*)buffer));
  addr   += 4;
  size   -= 4;
  buffer += 4;
}
if (size >0) {
    this->mem_read(addr&(~0x03), &wdata);
    memcpy(ptr, buffer, size);
    be = (1<<size) -1; //useless, but leave it here.
    retval = retval && this->mem_write(addr, be, wdata);
    printf("[2]addr =%x , v=%x", addr, wdata);

}
} else {
// read
printf("read %d bytes\n", size);
// bytes
if (addr & 0x1) {
be = addr&0x03;

  retval = retval && this->mem_read(addr, &rdata);
buffer[0] = ((char *)&rdata)[be];
  addr   += 1;
  size   -= 1;
  buffer += 1;
}
 
// half-words
if (addr & 0x2) {
  retval = retval && this->mem_read((addr&~0x3), &rdata);
  be = addr&0x03;
  buffer[0] = ((char*)&rdata)[be];
  buffer[1] = ((char*)&rdata)[be+1];
  addr   += 2;
  size   -= 2;
  buffer += 2;
}

while (size >= 4) {
  retval = retval && this->mem_read(addr, &rdata);
  buffer[0] = rdata;
  buffer[1] = rdata >> 8;
  buffer[2] = rdata >> 16;
  buffer[3] = rdata >> 24;
  addr   += 4;
  size   -= 4;
  buffer += 4;
}

// half-words
if (addr & 0x2) {
  retval = retval && this->mem_read(addr, &rdata);
  buffer[0] = rdata;
  buffer[1] = rdata >> 8;
  addr   += 2;
  size   -= 2;
  buffer += 2;
}

// bytes
if (addr & 0x1) {
  retval = retval && this->mem_read(addr, &rdata);
  buffer[0] = rdata;
  addr   += 1;
  size   -= 1;
  buffer += 1;
}

}

return retval;
}