@@ -47,26 +47,6 @@ void io_initialize()
4747 registers[2 ] = 3 ;
4848}
4949
50- void io_write_register (uint16_t address) {
51- openesc_register m;
52- m.set_address (address);
53- m.set_value (((uint8_t *)&g)[address]);
54- m.updateChecksum ();
55- // TODO this is not safe (non-copy)
56- io_write_message (&m);
57- }
58-
59- void io_write_register_multi (uint16_t address, uint16_t count) {
60- openesc_register_multi m (count);
61- m.set_address (address);
62- m.set_data_length (count);
63- for (uint16_t i = 0 ; i < m.data_length (); i++) {
64- m.set_data_at (i, ((uint8_t *)&g)[address + i]);
65- }
66- m.updateChecksum ();
67- io_write_message (&m);
68- }
69-
7050void io_handle_message (ping_message* message)
7151{
7252 switch (message->message_id ()) {
@@ -103,50 +83,6 @@ void io_handle_message(ping_message* message)
10383 }
10484 }
10585 break ;
106- case (OpenescId::SET_REGISTER):
107- {
108- openesc_set_register* m = (openesc_set_register*)message;
109- if (m->address () > sizeof (g)) {
110- io_write_message (&nack);
111- } else {
112- ((uint8_t *)&g)[m->address ()] = m->value ();
113- io_write_register (m->address ());
114- }
115- }
116- break ;
117- case (OpenescId::READ_REGISTER):
118- {
119- openesc_read_register* m = (openesc_read_register*)message;
120- if (m->address () > NUM_REGS) {
121- io_write_message (&nack);
122- } else {
123- io_write_register (m->address ());
124- }
125- }
126- break ;
127- case (OpenescId::READ_REGISTER_MULTI):
128- {
129- openesc_read_register_multi* m = (openesc_read_register_multi*)message;
130- if (m->address () + m->count () > sizeof (g)) {
131- io_write_message (&nack);
132- } else {
133- io_write_register_multi (m->address (), m->count ());
134- }
135- }
136- break ;
137- case (OpenescId::SET_REGISTER_MULTI):
138- {
139- openesc_set_register_multi* m = (openesc_set_register_multi*)message;
140- if (m->address () + m->data_length () > sizeof (g)) {
141- io_write_message (&nack);
142- } else {
143- for (uint16_t i = 0 ; i < m->data_length (); i++) {
144- ((uint8_t *)&g)[m->address () + i] = m->data ()[i];
145- }
146- io_write_register_multi (m->address (), m->data_length ());
147- }
148- }
149- break ;
15086 case (OpenescId::SET_THROTTLE):
15187 {
15288 openesc_set_throttle* m = (openesc_set_throttle*)message;
0 commit comments