1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
|
#include "bus.h"
#include <stdio.h>
#include <stdlib.h>
static device_vtable_t const bus_vtable = {
bus_read,
bus_write,
bus_deinit
};
void bus_init( bus_t *bus )
{
device_init( &bus->base, "bus" );
((device_t *)bus)->vtable = (device_vtable_t *)&bus_vtable;
bus->nof_devices = 0;
}
void bus_register( bus_t *bus, device_t *device, uint16_t from, uint16_t to )
{
if( bus->nof_devices < MAX_NOF_DEVICES ) {
registered_device_t *reg = &bus->devices[bus->nof_devices];
reg->device = device;
reg->from = from;
reg->to = to;
bus->nof_devices++;
} else {
fprintf( stderr, "ERROR: too many devices on bus\n" );
exit( EXIT_FAILURE );
}
}
uint8_t bus_read( void *obj, uint16_t addr )
{
bus_t *bus = (bus_t *)obj;
for( int i = 0; i < bus->nof_devices; i++ ) {
registered_device_t *reg = &bus->devices[i];
if( reg->from <= addr && addr <= reg->to ) {
return reg->device->vtable->read( reg->device, addr );
}
}
fprintf( stderr, "ERROR: illegal bus access reading from address %04X\n", addr );
return 0;
}
void bus_write( void *obj, uint16_t addr, uint8_t data )
{
bus_t *bus = (bus_t *)obj;
for( int i = 0; i < bus->nof_devices; i++ ) {
registered_device_t *reg = &bus->devices[i];
if( reg->from <= addr && addr <= reg->to ) {
return reg->device->vtable->write( reg->device, addr, data );
}
}
fprintf( stderr, "ERROR: illegal bus access writing from address %04X\n", addr );
}
void bus_deinit( void *obj )
{
bus_t *bus = (bus_t *)obj;
for( int i = bus->nof_devices - 1; i >= 0; i-- ) {
registered_device_t *reg = &bus->devices[i];
reg->device->vtable->deinit( reg->device );
}
bus->nof_devices = 0;
device_deinit( &bus->base );
}
|