/[dynamips]/upstream/dynamips-0.2.6-RC2/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

Annotation of /upstream/dynamips-0.2.6-RC2/dev_pcmcia_disk.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 1 - (hide annotations)
Sat Oct 6 16:01:44 2007 UTC (14 years, 8 months ago) by dpavlin
Original Path: upstream/dynamips-0.2.5/dev_pcmcia_disk.c
File MIME type: text/plain
File size: 14158 byte(s)
import 0.2.5 from upstream

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

  ViewVC Help
Powered by ViewVC 1.1.26