/[dynamips]/upstream/dynamips-0.2.6-RC3/dev_pcmcia_disk.c
This is repository of my old source code which isn't updated any more. Go to git.rot13.org for current projects!
ViewVC logotype

Contents of /upstream/dynamips-0.2.6-RC3/dev_pcmcia_disk.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 4 - (show annotations)
Sat Oct 6 16:06:49 2007 UTC (16 years, 5 months ago) by dpavlin
File MIME type: text/plain
File size: 17133 byte(s)
dynamips-0.2.6-RC3

1 /*
2 * Cisco C7200 (Predator) simulation platform.
3 * Copyright (c) 2006 Christophe Fillot. All rights reserved.
4 *
5 * PCMCIA ATA Flash emulation.
6 */
7
8 #include <stdio.h>
9 #include <stdlib.h>
10 #include <string.h>
11 #include <unistd.h>
12 #include <time.h>
13 #include <errno.h>
14 #include <sys/types.h>
15 #include <sys/stat.h>
16 #include <fcntl.h>
17
18 #include "mips64.h"
19 #include "dynamips.h"
20 #include "memory.h"
21 #include "device.h"
22
23 #define DEBUG_ACCESS 0
24 #define DEBUG_ATA 0
25 #define DEBUG_READ 0
26 #define DEBUG_WRITE 0
27
28 /* Default disk parameters: 4 heads, 32 sectors per track */
29 #define DISK_NR_HEADS 4
30 #define DISK_SECTS_PER_TRACK 32
31
32 /* Size (in bytes) of a sector */
33 #define SECTOR_SIZE 512
34
35 /* ATA commands */
36 #define ATA_CMD_NOP 0x00
37 #define ATA_CMD_READ_SECTOR 0x20
38 #define ATA_CMD_WRITE_SECTOR 0x30
39 #define ATA_CMD_IDENT_DEVICE 0xEC
40
41 /* ATA status */
42 #define ATA_STATUS_BUSY 0x80 /* Controller busy */
43 #define ATA_STATUS_RDY 0x40 /* Device ready */
44 #define ATA_STATUS_DWF 0x20 /* Write fault */
45 #define ATA_STATUS_DSC 0x10 /* Device ready */
46 #define ATA_STATUS_DRQ 0x08 /* Data Request */
47 #define ATA_STATUS_CORR 0x04 /* Correctable error */
48 #define ATA_STATUS_IDX 0x02 /* Always 0 */
49 #define ATA_STATUS_ERR 0x01 /* Error */
50
51 /* ATA Drive/Head register */
52 #define ATA_DH_LBA 0x40 /* LBA Mode */
53
54 /* Card Information Structure */
55 static m_uint8_t cis_table[] = {
56 0x01, 0x03, 0xd9, 0x01, 0xff, 0x1c, 0x04, 0x03,
57 0xd9, 0x01, 0xff, 0x18, 0x02, 0xdf, 0x01, 0x20,
58 0x04, 0x34, 0x12, 0x00, 0x02, 0x15, 0x2b, 0x04,
59 0x01, 0x44, 0x79, 0x6e, 0x61, 0x6d, 0x69, 0x70,
60 0x73, 0x20, 0x41, 0x54, 0x41, 0x20, 0x46, 0x6c,
61 0x61, 0x73, 0x68, 0x20, 0x43, 0x61, 0x72, 0x64,
62 0x20, 0x20, 0x00, 0x44, 0x59, 0x4e, 0x41, 0x30,
63 0x20, 0x20, 0x00, 0x44, 0x59, 0x4e, 0x41, 0x30,
64 0x00, 0xff, 0x21, 0x02, 0x04, 0x01, 0x22, 0x02,
65 0x01, 0x01, 0x22, 0x03, 0x02, 0x04, 0x5f, 0x1a,
66 0x05, 0x01, 0x03, 0x00, 0x02, 0x0f, 0x1b, 0x0b,
67 0xc0, 0x40, 0xa1, 0x27, 0x55, 0x4d, 0x5d, 0x75,
68 0x08, 0x00, 0x21, 0x1b, 0x06, 0x00, 0x01, 0x21,
69 0xb5, 0x1e, 0x4d, 0x1b, 0x0d, 0xc1, 0x41, 0x99,
70 0x27, 0x55, 0x4d, 0x5d, 0x75, 0x64, 0xf0, 0xff,
71 0xff, 0x21, 0x1b, 0x06, 0x01, 0x01, 0x21, 0xb5,
72 0x1e, 0x4d, 0x1b, 0x12, 0xc2, 0x41, 0x99, 0x27,
73 0x55, 0x4d, 0x5d, 0x75, 0xea, 0x61, 0xf0, 0x01,
74 0x07, 0xf6, 0x03, 0x01, 0xee, 0x21, 0x1b, 0x06,
75 0x02, 0x01, 0x21, 0xb5, 0x1e, 0x4d, 0x1b, 0x12,
76 0xc3, 0x41, 0x99, 0x27, 0x55, 0x4d, 0x5d, 0x75,
77 0xea, 0x61, 0x70, 0x01, 0x07, 0x76, 0x03, 0x01,
78 0xee, 0x21, 0x1b, 0x06, 0x03, 0x01, 0x21, 0xb5,
79 0x1e, 0x4d, 0x14, 0x00,
80 };
81
82 /* PCMCIA private data */
83 struct pcmcia_disk_data {
84 vm_instance_t *vm;
85 vm_obj_t vm_obj;
86 struct vdevice dev;
87 char *filename;
88 int fd;
89
90 /* Disk parameters (C/H/S) */
91 u_int nr_heads;
92 u_int nr_cylinders;
93 u_int sects_per_track;
94
95 /* Current ATA command and CHS info */
96 m_uint8_t ata_cmd,ata_cmd_in_progress;
97 m_uint8_t ata_status;
98
99 m_uint8_t cyl_low,cyl_high;
100 m_uint8_t head,sect_no;
101 m_uint8_t sect_count;
102
103 /* Current sector */
104 m_uint32_t sect_pos;
105
106 /* Remaining sectors to read or write */
107 u_int sect_remaining;
108
109 /* Callback function when data buffer is validated */
110 void (*ata_cmd_callback)(struct pcmcia_disk_data *);
111
112 /* Data buffer */
113 m_uint32_t data_offset;
114 u_int data_pos;
115 m_uint8_t data_buffer[SECTOR_SIZE];
116 };
117
118 /* Convert a CHS reference to an LBA reference */
119 static inline m_uint32_t chs_to_lba(struct pcmcia_disk_data *d,
120 u_int cyl,u_int head,u_int sect)
121 {
122 return((((cyl * d->nr_heads) + head) * d->sects_per_track) + sect - 1);
123 }
124
125 /* Create the virtual disk */
126 static int disk_create(struct pcmcia_disk_data *d)
127 {
128 off_t disk_len;
129
130 if ((d->fd = open(d->filename,O_CREAT|O_RDWR,0600)) < 0) {
131 perror("disk_create: open");
132 return(-1);
133 }
134
135 disk_len = d->nr_heads * d->nr_cylinders * d->sects_per_track * SECTOR_SIZE;
136 ftruncate(d->fd,disk_len);
137 return(0);
138 }
139
140 /* Read a sector from disk file */
141 static int disk_read_sector(struct pcmcia_disk_data *d,m_uint32_t sect,
142 m_uint8_t *buffer)
143 {
144 off_t disk_offset = (off_t)sect * SECTOR_SIZE;
145
146 #if DEBUG_READ
147 vm_log(d->vm,d->dev.name,"reading sector 0x%8.8x\n",sect);
148 #endif
149
150 if (lseek(d->fd,disk_offset,SEEK_SET) == -1) {
151 perror("read_sector: lseek");
152 return(-1);
153 }
154
155 if (read(d->fd,buffer,SECTOR_SIZE) != SECTOR_SIZE) {
156 perror("read_sector: read");
157 return(-1);
158 }
159
160 return(0);
161 }
162
163 /* Write a sector to disk file */
164 static int disk_write_sector(struct pcmcia_disk_data *d,m_uint32_t sect,
165 m_uint8_t *buffer)
166 {
167 off_t disk_offset = (off_t)sect * SECTOR_SIZE;
168
169 #if DEBUG_WRITE
170 vm_log(d->vm,d->dev.name,"writing sector 0x%8.8x\n",sect);
171 #endif
172
173 if (lseek(d->fd,disk_offset,SEEK_SET) == -1) {
174 perror("write_sector: lseek");
175 return(-1);
176 }
177
178 if (write(d->fd,buffer,SECTOR_SIZE) != SECTOR_SIZE) {
179 perror("write_sector: write");
180 return(-1);
181 }
182
183 return(0);
184 }
185
186 /* Identify PCMCIA device (ATA command 0xEC) */
187 static void ata_identify_device(struct pcmcia_disk_data *d)
188 {
189 m_uint8_t *p = d->data_buffer;
190 m_uint32_t sect_count;
191
192 sect_count = d->nr_heads * d->nr_cylinders * d->sects_per_track;
193
194 /* Clear all fields (for safety) */
195 memset(p,0x00,SECTOR_SIZE);
196
197 /* Word 0: General Configuration */
198 p[0] = 0x8a;
199 p[1] = 0x84;
200
201 /* Word 1: Default number of cylinders */
202 p[2] = d->nr_cylinders & 0xFF;
203 p[3] = (d->nr_cylinders >> 8) & 0xFF;
204
205 /* Word 3: Default number of heads */
206 p[6] = d->nr_heads;
207
208 /* Word 6: Default number of sectors per track */
209 p[12] = d->sects_per_track;
210
211 /* Word 7: Number of sectors per card (MSW) */
212 p[14] = (sect_count >> 16) & 0xFF;
213 p[15] = (sect_count >> 24);
214
215 /* Word 8: Number of sectors per card (LSW) */
216 p[16] = sect_count & 0xFF;
217 p[17] = (sect_count >> 8) & 0xFF;
218
219 /* Word 22: ECC count */
220 p[44] = 0x04;
221
222 /* Word 53: Translation parameters valid */
223 p[106] = 0x3;
224
225 /* Word 54: Current number of cylinders */
226 p[108] = d->nr_cylinders & 0xFF;
227 p[109] = (d->nr_cylinders >> 8) & 0xFF;
228
229 /* Word 55: Current number of heads */
230 p[110] = d->nr_heads;
231
232 /* Word 56: Current number of sectors per track */
233 p[112] = d->sects_per_track;
234
235 /* Word 57/58: Current of sectors per card (LSW/MSW) */
236 p[114] = sect_count & 0xFF;
237 p[115] = (sect_count >> 8) & 0xFF;
238
239 p[116] = (sect_count >> 16) & 0xFF;
240 p[117] = (sect_count >> 24);
241
242 #if 0
243 /* Word 60/61: Total sectors addressable in LBA mode (MSW/LSW) */
244 p[120] = (sect_count >> 16) & 0xFF;
245 p[121] = (sect_count >> 24);
246 p[122] = sect_count & 0xFF;
247 p[123] = (sect_count >> 8) & 0xFF;
248 #endif
249 }
250
251 /* Set sector position */
252 static void ata_set_sect_pos(struct pcmcia_disk_data *d)
253 {
254 u_int cyl;
255
256 if (d->head & ATA_DH_LBA) {
257 d->sect_pos = (u_int)(d->head & 0x0F) << 24;
258 d->sect_pos |= (u_int)d->cyl_high << 16;
259 d->sect_pos |= (u_int)d->cyl_low << 8;
260 d->sect_pos |= (u_int)d->sect_no;
261
262 #if DEBUG_ATA
263 vm_log(d->vm,d->dev.name,"ata_set_sect_pos: LBA sect=0x%x\n",
264 d->sect_pos);
265 #endif
266 } else {
267 cyl = (((u_int)d->cyl_high) << 8) + d->cyl_low;
268 d->sect_pos = chs_to_lba(d,cyl,d->head & 0x0F,d->sect_no);
269
270 #if DEBUG_ATA
271 vm_log(d->vm,d->dev.name,
272 "ata_set_sect_pos: cyl=0x%x,head=0x%x,sect=0x%x => "
273 "sect_pos=0x%x\n",
274 cyl,d->head & 0x0F,d->sect_no,d->sect_pos);
275 #endif
276 }
277 }
278
279 /* ATA device identifier callback */
280 static void ata_cmd_ident_device_callback(struct pcmcia_disk_data *d)
281 {
282 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC;
283 }
284
285 /* ATA read sector callback */
286 static void ata_cmd_read_callback(struct pcmcia_disk_data *d)
287 {
288 d->sect_remaining--;
289
290 if (!d->sect_remaining) {
291 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC;
292 return;
293 }
294
295 /* Read the next sector */
296 d->sect_pos++;
297 disk_read_sector(d,d->sect_pos,d->data_buffer);
298 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC|ATA_STATUS_DRQ;
299 }
300
301 /* ATA write sector callback */
302 static void ata_cmd_write_callback(struct pcmcia_disk_data *d)
303 {
304 /* Write the sector */
305 disk_write_sector(d,d->sect_pos,d->data_buffer);
306 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC|ATA_STATUS_DRQ;
307 d->sect_pos++;
308
309 d->sect_remaining--;
310
311 if (!d->sect_remaining) {
312 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC;
313 }
314 }
315
316 /* Handle an ATA command */
317 static void ata_handle_cmd(struct pcmcia_disk_data *d)
318 {
319 #if DEBUG_ATA
320 vm_log(d->vm,d->dev.name,"ATA command 0x%2.2x\n",(u_int)d->ata_cmd);
321 #endif
322
323 d->data_pos = 0;
324
325 switch(d->ata_cmd) {
326 case ATA_CMD_IDENT_DEVICE:
327 ata_identify_device(d);
328 d->ata_cmd_callback = ata_cmd_ident_device_callback;
329 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC|ATA_STATUS_DRQ;
330 break;
331
332 case ATA_CMD_READ_SECTOR:
333 d->sect_remaining = d->sect_count;
334
335 if (!d->sect_remaining)
336 d->sect_remaining = 256;
337
338 ata_set_sect_pos(d);
339 disk_read_sector(d,d->sect_pos,d->data_buffer);
340 d->ata_cmd_callback = ata_cmd_read_callback;
341 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC|ATA_STATUS_DRQ;
342 break;
343
344 case ATA_CMD_WRITE_SECTOR:
345 d->sect_remaining = d->sect_count;
346
347 if (!d->sect_remaining)
348 d->sect_remaining = 256;
349
350 ata_set_sect_pos(d);
351 d->ata_cmd_callback = ata_cmd_write_callback;
352 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC|ATA_STATUS_DRQ;
353 break;
354
355 default:
356 vm_log(d->vm,d->dev.name,"unhandled ATA command 0x%2.2x\n",
357 (u_int)d->ata_cmd);
358 }
359 }
360
361 /*
362 * dev_pcmcia_disk_access_0()
363 */
364 void *dev_pcmcia_disk_access_0(cpu_mips_t *cpu,struct vdevice *dev,
365 m_uint32_t offset,u_int op_size,u_int op_type,
366 m_uint64_t *data)
367 {
368 struct pcmcia_disk_data *d = dev->priv_data;
369
370 /* Compute the good internal offset */
371 offset = (offset >> 1) ^ 1;
372
373 #if DEBUG_ACCESS
374 if (op_type == MTS_READ) {
375 cpu_log(cpu,d->dev.name,
376 "reading offset 0x%5.5x at pc=0x%llx (size=%u)\n",
377 offset,cpu->pc,op_size);
378 } else {
379 cpu_log(cpu,d->dev.name,
380 "writing offset 0x%5.5x, data=0x%llx at pc=0x%llx (size=%u)\n",
381 offset,*data,cpu->pc,op_size);
382 }
383 #endif
384
385 /* Card Information Structure */
386 if (offset < sizeof(cis_table)) {
387 if (op_type == MTS_READ)
388 *data = cis_table[offset];
389
390 return NULL;
391 }
392
393 switch(offset) {
394 case 0x102: /* Pin Replacement Register */
395 if (op_type == MTS_READ)
396 *data = 0x22;
397 break;
398
399 case 0x80001: /* Sector Count + Sector no */
400 if (op_type == MTS_READ) {
401 *data = (d->sect_no << 8) + d->sect_count;
402 } else {
403 d->sect_no = *data >> 8;
404 d->sect_count = *data & 0xFF;
405 }
406 break;
407
408 case 0x80002: /* Cylinder Low + Cylinder High */
409 if (op_type == MTS_READ) {
410 *data = (d->cyl_high << 8) + d->cyl_low;
411 } else {
412 d->cyl_high = *data >> 8;
413 d->cyl_low = *data & 0xFF;
414 }
415 break;
416
417 case 0x80003: /* Select Card/Head + Status/Command register */
418 if (op_type == MTS_READ)
419 *data = (d->ata_status << 8) + d->head;
420 else {
421 d->ata_cmd = *data >> 8;
422 d->head = *data;
423 ata_handle_cmd(d);
424 }
425 break;
426
427 default:
428 /* Data buffer access ? */
429 if ((offset >= d->data_offset) &&
430 (offset < d->data_offset + (SECTOR_SIZE/2)))
431 {
432 if (op_type == MTS_READ) {
433 *data = d->data_buffer[(d->data_pos << 1)];
434 *data += d->data_buffer[(d->data_pos << 1)+1] << 8;
435 } else {
436 d->data_buffer[(d->data_pos << 1)] = *data & 0xFF;
437 d->data_buffer[(d->data_pos << 1)+1] = *data >> 8;
438 }
439
440 d->data_pos++;
441
442 /* Buffer complete: call the callback function */
443 if (d->data_pos == (SECTOR_SIZE/2)) {
444 d->data_pos = 0;
445
446 if (d->ata_cmd_callback)
447 d->ata_cmd_callback(d);
448 }
449 }
450 }
451
452 return NULL;
453 }
454
455 /*
456 * dev_pcmcia_disk_access_1()
457 */
458 void *dev_pcmcia_disk_access_1(cpu_mips_t *cpu,struct vdevice *dev,
459 m_uint32_t offset,u_int op_size,u_int op_type,
460 m_uint64_t *data)
461 {
462 struct pcmcia_disk_data *d = dev->priv_data;
463
464 /* Compute the good internal offset */
465 offset = (offset >> 1) ^ 1;
466
467 #if DEBUG_ACCESS
468 if (op_type == MTS_READ) {
469 cpu_log(cpu,d->dev.name,
470 "reading offset 0x%5.5x at pc=0x%llx (size=%u)\n",
471 offset,cpu->pc,op_size);
472 } else {
473 cpu_log(cpu,d->dev.name,
474 "writing offset 0x%5.5x, data=0x%llx at pc=0x%llx (size=%u)\n",
475 offset,*data,cpu->pc,op_size);
476 }
477 #endif
478
479 switch(offset) {
480 case 0x02: /* Sector Count + Sector no */
481 if (op_type == MTS_READ) {
482 *data = (d->sect_no << 8) + d->sect_count;
483 } else {
484 d->sect_no = *data >> 8;
485 d->sect_count = *data & 0xFF;
486 }
487 break;
488
489 case 0x04: /* Cylinder Low + Cylinder High */
490 if (op_type == MTS_READ) {
491 *data = (d->cyl_high << 8) + d->cyl_low;
492 } else {
493 d->cyl_high = *data >> 8;
494 d->cyl_low = *data & 0xFF;
495 }
496 break;
497
498 case 0x06: /* Select Card/Head + Status/Command register */
499 if (op_type == MTS_READ)
500 *data = (d->ata_status << 8) + d->head;
501 else {
502 d->ata_cmd = *data >> 8;
503 d->head = *data & 0xFF;
504 ata_handle_cmd(d);
505 }
506 break;
507
508 case 0x08:
509 if (op_type == MTS_READ) {
510 *data = d->data_buffer[(d->data_pos << 1)];
511 *data += d->data_buffer[(d->data_pos << 1)+1] << 8;
512 } else {
513 d->data_buffer[(d->data_pos << 1)] = *data & 0xFF;
514 d->data_buffer[(d->data_pos << 1)+1] = *data >> 8;
515 }
516
517 d->data_pos++;
518
519 /* Buffer complete: call the callback function */
520 if (d->data_pos == (SECTOR_SIZE/2)) {
521 d->data_pos = 0;
522
523 if (d->ata_cmd_callback)
524 d->ata_cmd_callback(d);
525 }
526 break;
527
528 case 0x0E:
529 break;
530 }
531
532 return NULL;
533 }
534
535 /* Shutdown a PCMCIA disk device */
536 void dev_pcmcia_disk_shutdown(vm_instance_t *vm,struct pcmcia_disk_data *d)
537 {
538 if (d != NULL) {
539 /* Remove the device */
540 dev_remove(vm,&d->dev);
541
542 /* Close disk file */
543 if (d->fd != -1) close(d->fd);
544
545 /* Free filename */
546 free(d->filename);
547
548 /* Free the structure itself */
549 free(d);
550 }
551 }
552
553 /* Initialize a PCMCIA disk */
554 vm_obj_t *dev_pcmcia_disk_init(vm_instance_t *vm,char *name,
555 m_uint64_t paddr,m_uint32_t len,
556 u_int disk_size,int mode)
557 {
558 struct pcmcia_disk_data *d;
559 m_uint32_t tot_sect;
560
561 /* allocate the private data structure */
562 if (!(d = malloc(sizeof(*d)))) {
563 fprintf(stderr,"PCMCIA: unable to create disk device '%s'.\n",name);
564 return NULL;
565 }
566
567 memset(d,0,sizeof(*d));
568 vm_object_init(&d->vm_obj);
569 d->vm = vm;
570 d->vm_obj.name = name;
571 d->vm_obj.data = d;
572 d->vm_obj.shutdown = (vm_shutdown_t)dev_pcmcia_disk_shutdown;
573 d->fd = -1;
574
575 if (!(d->filename = vm_build_filename(vm,name))) {
576 fprintf(stderr,"PCMCIA: unable to create filename.\n");
577 goto err_filename;
578 }
579
580 /* Data buffer offset in mapped memory */
581 d->data_offset = 0x80200;
582 d->ata_status = ATA_STATUS_RDY|ATA_STATUS_DSC;
583
584 /* Compute the number of cylinders given a disk size in Mb */
585 tot_sect = ((m_uint64_t)disk_size * 1048576) / SECTOR_SIZE;
586
587 d->nr_heads = DISK_NR_HEADS;
588 d->sects_per_track = DISK_SECTS_PER_TRACK;
589 d->nr_cylinders = tot_sect / (d->nr_heads * d->sects_per_track);
590
591 vm_log(vm,name,"C/H/S settings = %u/%u/%u\n",
592 d->nr_cylinders,d->nr_heads,d->sects_per_track);
593
594 /* Create the disk file */
595 if (disk_create(d) == -1)
596 goto err_disk_create;
597
598 dev_init(&d->dev);
599 d->dev.name = name;
600 d->dev.priv_data = d;
601 d->dev.phys_addr = paddr;
602 d->dev.phys_len = len;
603 d->dev.flags = VDEVICE_FLAG_CACHING;
604
605 if (mode == 0)
606 d->dev.handler = dev_pcmcia_disk_access_0;
607 else
608 d->dev.handler = dev_pcmcia_disk_access_1;
609
610 /* Map this device to the VM */
611 vm_bind_device(vm,&d->dev);
612 vm_object_add(vm,&d->vm_obj);
613 return(&d->vm_obj);
614
615 err_disk_create:
616 free(d->filename);
617 err_filename:
618 free(d);
619 return NULL;
620 }

  ViewVC Help
Powered by ViewVC 1.1.26