gwj / at91sam9263 · Commits

Commit 9f149aa9c1a04117d03de16bb0f8d2dad8c4f8c1

Authored by pbrook
1 parent 7d8406be

SATN fixes (Blue Swirl). 


git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@1947 c046a42c-6fe2-441c-8c8c-71466251a162
Inline Side-by-side
Showing 1 changed file with 75 additions and 18 deletions
hw/esp.c
  View file @9f149aa
... ... @@ -59,6 +59,9 @@ struct ESPState {
59 59 int dma;
60 60 SCSIDevice *scsi_dev[MAX_DISKS];
61 61 SCSIDevice *current_dev;
  62 + uint8_t cmdbuf[TI_BUFSZ];
  63 + int cmdlen;
  64 + int do_cmd;
62 65 };
63 66  
64 67 #define STAT_DO 0x00
... ... @@ -79,17 +82,14 @@ struct ESPState {
79 82 #define SEQ_0 0x0
80 83 #define SEQ_CD 0x4
81 84  
82   -static void handle_satn(ESPState *s)
  85 +static int get_cmd(ESPState *s, uint8_t *buf)
83 86 {
84   - uint8_t buf[32];
85 87 uint32_t dmaptr, dmalen;
86 88 int target;
87   - int32_t datalen;
88   - int lun;
89 89  
90 90 dmalen = s->wregs[0] | (s->wregs[1] << 8);
91 91 target = s->wregs[4] & 7;
92   - DPRINTF("Select with ATN len %d target %d\n", dmalen, target);
  92 + DPRINTF("get_cmd: len %d target %d\n", dmalen, target);
93 93 if (s->dma) {
94 94 dmaptr = iommu_translate(s->espdmaregs[1]);
95 95 DPRINTF("DMA Direction: %c, addr 0x%8.8x\n",
... ... @@ -100,8 +100,6 @@ static void handle_satn(ESPState *s)
100 100 memcpy(&buf[1], s->ti_buf, dmalen);
101 101 dmalen++;
102 102 }
103   - DPRINTF("busid 0x%x\n", buf[0]);
104   - lun = buf[0] & 7;
105 103  
106 104 s->ti_size = 0;
107 105 s->ti_rptr = 0;
... ... @@ -114,9 +112,19 @@ static void handle_satn(ESPState *s)
114 112 s->rregs[6] = SEQ_0;
115 113 s->espdmaregs[0] |= DMA_INTR;
116 114 pic_set_irq(s->irq, 1);
117   - return;
  115 + return 0;
118 116 }
119 117 s->current_dev = s->scsi_dev[target];
  118 + return dmalen;
  119 +}
  120 +
  121 +static void do_cmd(ESPState *s, uint8_t *buf)
  122 +{
  123 + int32_t datalen;
  124 + int lun;
  125 +
  126 + DPRINTF("do_cmd: busid 0x%x\n", buf[0]);
  127 + lun = buf[0] & 7;
120 128 datalen = scsi_send_command(s->current_dev, 0, &buf[1], lun);
121 129 if (datalen == 0) {
122 130 s->ti_size = 0;
... ... @@ -136,6 +144,31 @@ static void handle_satn(ESPState *s)
136 144 pic_set_irq(s->irq, 1);
137 145 }
138 146  
  147 +static void handle_satn(ESPState *s)
  148 +{
  149 + uint8_t buf[32];
  150 + int len;
  151 +
  152 + len = get_cmd(s, buf);
  153 + if (len)
  154 + do_cmd(s, buf);
  155 +}
  156 +
  157 +static void handle_satn_stop(ESPState *s)
  158 +{
  159 + s->cmdlen = get_cmd(s, s->cmdbuf);
  160 + if (s->cmdlen) {
  161 + DPRINTF("Set ATN & Stop: cmdlen %d\n", s->cmdlen);
  162 + s->do_cmd = 1;
  163 + s->espdmaregs[1] += s->cmdlen;
  164 + s->rregs[4] = STAT_IN | STAT_TC | STAT_CD;
  165 + s->rregs[5] = INTR_BS | INTR_FC;
  166 + s->rregs[6] = SEQ_CD;
  167 + s->espdmaregs[0] |= DMA_INTR;
  168 + pic_set_irq(s->irq, 1);
  169 + }
  170 +}
  171 +
139 172 static void write_response(ESPState *s)
140 173 {
141 174 uint32_t dmaptr;
... ... @@ -188,7 +221,10 @@ static void handle_ti(ESPState *s)
188 221 dmalen=0x10000;
189 222 }
190 223  
191   - minlen = (dmalen < s->ti_size) ? dmalen : s->ti_size;
  224 + if (s->do_cmd)
  225 + minlen = (dmalen < 32) ? dmalen : 32;
  226 + else
  227 + minlen = (dmalen < s->ti_size) ? dmalen : s->ti_size;
192 228 DPRINTF("Transfer Information len %d\n", minlen);
193 229 if (s->dma) {
194 230 dmaptr = iommu_translate(s->espdmaregs[1]);
... ... @@ -207,14 +243,24 @@ static void handle_ti(ESPState *s)
207 243 }
208 244 DPRINTF("DMA address p %08x v %08x len %08x, from %08x, to %08x\n", dmaptr, s->espdmaregs[1] + i, len, from, to);
209 245 s->ti_size -= len;
210   - if (to_device) {
211   - cpu_physical_memory_read(dmaptr, buf, len);
212   - scsi_write_data(s->current_dev, buf, len);
  246 + if (s->do_cmd) {
  247 + DPRINTF("command len %d + %d\n", s->cmdlen, len);
  248 + cpu_physical_memory_read(dmaptr, &s->cmdbuf[s->cmdlen], len);
  249 + s->ti_size = 0;
  250 + s->cmdlen = 0;
  251 + s->do_cmd = 0;
  252 + do_cmd(s, s->cmdbuf);
  253 + return;
213 254 } else {
214   - scsi_read_data(s->current_dev, buf, len);
215   - cpu_physical_memory_write(dmaptr, buf, len);
  255 + if (to_device) {
  256 + cpu_physical_memory_read(dmaptr, buf, len);
  257 + scsi_write_data(s->current_dev, buf, len);
  258 + } else {
  259 + scsi_read_data(s->current_dev, buf, len);
  260 + cpu_physical_memory_write(dmaptr, buf, len);
  261 + }
216 262 }
217   - }
  263 + }
218 264 if (s->ti_size) {
219 265 s->rregs[4] = STAT_IN | STAT_TC | (to_device ? STAT_DO : STAT_DI);
220 266 }
... ... @@ -222,7 +268,14 @@ static void handle_ti(ESPState *s)
222 268 s->rregs[6] = 0;
223 269 s->rregs[7] = 0;
224 270 s->espdmaregs[0] |= DMA_INTR;
225   - }
  271 + } else if (s->do_cmd) {
  272 + DPRINTF("command len %d\n", s->cmdlen);
  273 + s->ti_size = 0;
  274 + s->cmdlen = 0;
  275 + s->do_cmd = 0;
  276 + do_cmd(s, s->cmdbuf);
  277 + return;
  278 + }
226 279 pic_set_irq(s->irq, 1);
227 280 }
228 281  
... ... @@ -237,6 +290,7 @@ static void esp_reset(void *opaque)
237 290 s->ti_rptr = 0;
238 291 s->ti_wptr = 0;
239 292 s->dma = 0;
  293 + s->do_cmd = 0;
240 294 }
241 295  
242 296 static uint32_t esp_mem_readb(void *opaque, target_phys_addr_t addr)
... ... @@ -291,7 +345,9 @@ static void esp_mem_writeb(void *opaque, target_phys_addr_t addr, uint32_t val)
291 345 break;
292 346 case 2:
293 347 // FIFO
294   - if ((s->rregs[4] & 6) == 0) {
  348 + if (s->do_cmd) {
  349 + s->cmdbuf[s->cmdlen++] = val & 0xff;
  350 + } else if ((s->rregs[4] & 6) == 0) {
295 351 uint8_t buf;
296 352 buf = val & 0xff;
297 353 s->ti_size--;
... ... @@ -348,11 +404,12 @@ static void esp_mem_writeb(void *opaque, target_phys_addr_t addr, uint32_t val)
348 404 DPRINTF("Set ATN (%2.2x)\n", val);
349 405 break;
350 406 case 0x42:
  407 + DPRINTF("Set ATN (%2.2x)\n", val);
351 408 handle_satn(s);
352 409 break;
353 410 case 0x43:
354 411 DPRINTF("Set ATN & stop (%2.2x)\n", val);
355   - handle_satn(s);
  412 + handle_satn_stop(s);
356 413 break;
357 414 default:
358 415 DPRINTF("Unhandled ESP command (%2.2x)\n", val);
... ...