/[dynamips]/upstream/dynamips-0.2.7-RC1/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.7-RC1/dev_pcmcia_disk.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 7 - (show annotations)
Sat Oct 6 16:23:47 2007 UTC (12 years, 1 month ago) by dpavlin
File MIME type: text/plain
File size: 17376 byte(s)
dynamips-0.2.7-RC1

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

  ViewVC Help
Powered by ViewVC 1.1.26