diff options
Diffstat (limited to 'quantum/serial_link/protocol/transport.c')
| -rw-r--r-- | quantum/serial_link/protocol/transport.c | 61 |
1 files changed, 27 insertions, 34 deletions
diff --git a/quantum/serial_link/protocol/transport.c b/quantum/serial_link/protocol/transport.c index ff795fe20..73b8dc62e 100644 --- a/quantum/serial_link/protocol/transport.c +++ b/quantum/serial_link/protocol/transport.c | |||
| @@ -29,42 +29,38 @@ SOFTWARE. | |||
| 29 | 29 | ||
| 30 | #define MAX_REMOTE_OBJECTS 16 | 30 | #define MAX_REMOTE_OBJECTS 16 |
| 31 | static remote_object_t* remote_objects[MAX_REMOTE_OBJECTS]; | 31 | static remote_object_t* remote_objects[MAX_REMOTE_OBJECTS]; |
| 32 | static uint32_t num_remote_objects = 0; | 32 | static uint32_t num_remote_objects = 0; |
| 33 | 33 | ||
| 34 | void reinitialize_serial_link_transport(void) { | 34 | void reinitialize_serial_link_transport(void) { num_remote_objects = 0; } |
| 35 | num_remote_objects = 0; | ||
| 36 | } | ||
| 37 | 35 | ||
| 38 | void add_remote_objects(remote_object_t** _remote_objects, uint32_t _num_remote_objects) { | 36 | void add_remote_objects(remote_object_t** _remote_objects, uint32_t _num_remote_objects) { |
| 39 | unsigned int i; | 37 | unsigned int i; |
| 40 | for(i=0;i<_num_remote_objects;i++) { | 38 | for (i = 0; i < _num_remote_objects; i++) { |
| 41 | remote_object_t* obj = _remote_objects[i]; | 39 | remote_object_t* obj = _remote_objects[i]; |
| 42 | remote_objects[num_remote_objects++] = obj; | 40 | remote_objects[num_remote_objects++] = obj; |
| 43 | if (obj->object_type == MASTER_TO_ALL_SLAVES) { | 41 | if (obj->object_type == MASTER_TO_ALL_SLAVES) { |
| 44 | triple_buffer_object_t* tb = (triple_buffer_object_t*)obj->buffer; | 42 | triple_buffer_object_t* tb = (triple_buffer_object_t*)obj->buffer; |
| 45 | triple_buffer_init(tb); | 43 | triple_buffer_init(tb); |
| 46 | uint8_t* start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size); | 44 | uint8_t* start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size); |
| 47 | tb = (triple_buffer_object_t*)start; | 45 | tb = (triple_buffer_object_t*)start; |
| 48 | triple_buffer_init(tb); | 46 | triple_buffer_init(tb); |
| 49 | } | 47 | } else if (obj->object_type == MASTER_TO_SINGLE_SLAVE) { |
| 50 | else if(obj->object_type == MASTER_TO_SINGLE_SLAVE) { | 48 | uint8_t* start = obj->buffer; |
| 51 | uint8_t* start = obj->buffer; | ||
| 52 | unsigned int j; | 49 | unsigned int j; |
| 53 | for (j=0;j<NUM_SLAVES;j++) { | 50 | for (j = 0; j < NUM_SLAVES; j++) { |
| 54 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; | 51 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; |
| 55 | triple_buffer_init(tb); | 52 | triple_buffer_init(tb); |
| 56 | start += LOCAL_OBJECT_SIZE(obj->object_size); | 53 | start += LOCAL_OBJECT_SIZE(obj->object_size); |
| 57 | } | 54 | } |
| 58 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; | 55 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; |
| 59 | triple_buffer_init(tb); | 56 | triple_buffer_init(tb); |
| 60 | } | 57 | } else { |
| 61 | else { | 58 | uint8_t* start = obj->buffer; |
| 62 | uint8_t* start = obj->buffer; | 59 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; |
| 63 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; | ||
| 64 | triple_buffer_init(tb); | 60 | triple_buffer_init(tb); |
| 65 | start += LOCAL_OBJECT_SIZE(obj->object_size); | 61 | start += LOCAL_OBJECT_SIZE(obj->object_size); |
| 66 | unsigned int j; | 62 | unsigned int j; |
| 67 | for (j=0;j<NUM_SLAVES;j++) { | 63 | for (j = 0; j < NUM_SLAVES; j++) { |
| 68 | tb = (triple_buffer_object_t*)start; | 64 | tb = (triple_buffer_object_t*)start; |
| 69 | triple_buffer_init(tb); | 65 | triple_buffer_init(tb); |
| 70 | start += REMOTE_OBJECT_SIZE(obj->object_size); | 66 | start += REMOTE_OBJECT_SIZE(obj->object_size); |
| @@ -74,23 +70,21 @@ void add_remote_objects(remote_object_t** _remote_objects, uint32_t _num_remote_ | |||
| 74 | } | 70 | } |
| 75 | 71 | ||
| 76 | void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size) { | 72 | void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size) { |
| 77 | uint8_t id = data[size-1]; | 73 | uint8_t id = data[size - 1]; |
| 78 | if (id < num_remote_objects) { | 74 | if (id < num_remote_objects) { |
| 79 | remote_object_t* obj = remote_objects[id]; | 75 | remote_object_t* obj = remote_objects[id]; |
| 80 | if (obj->object_size == size - 1) { | 76 | if (obj->object_size == size - 1) { |
| 81 | uint8_t* start; | 77 | uint8_t* start; |
| 82 | if (obj->object_type == MASTER_TO_ALL_SLAVES) { | 78 | if (obj->object_type == MASTER_TO_ALL_SLAVES) { |
| 83 | start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size); | 79 | start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size); |
| 84 | } | 80 | } else if (obj->object_type == SLAVE_TO_MASTER) { |
| 85 | else if(obj->object_type == SLAVE_TO_MASTER) { | ||
| 86 | start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size); | 81 | start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size); |
| 87 | start += (from - 1) * REMOTE_OBJECT_SIZE(obj->object_size); | 82 | start += (from - 1) * REMOTE_OBJECT_SIZE(obj->object_size); |
| 88 | } | 83 | } else { |
| 89 | else { | ||
| 90 | start = obj->buffer + NUM_SLAVES * LOCAL_OBJECT_SIZE(obj->object_size); | 84 | start = obj->buffer + NUM_SLAVES * LOCAL_OBJECT_SIZE(obj->object_size); |
| 91 | } | 85 | } |
| 92 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; | 86 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; |
| 93 | void* ptr = triple_buffer_begin_write_internal(obj->object_size, tb); | 87 | void* ptr = triple_buffer_begin_write_internal(obj->object_size, tb); |
| 94 | memcpy(ptr, data, size - 1); | 88 | memcpy(ptr, data, size - 1); |
| 95 | triple_buffer_end_write_internal(tb); | 89 | triple_buffer_end_write_internal(tb); |
| 96 | } | 90 | } |
| @@ -99,26 +93,25 @@ void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size) { | |||
| 99 | 93 | ||
| 100 | void update_transport(void) { | 94 | void update_transport(void) { |
| 101 | unsigned int i; | 95 | unsigned int i; |
| 102 | for(i=0;i<num_remote_objects;i++) { | 96 | for (i = 0; i < num_remote_objects; i++) { |
| 103 | remote_object_t* obj = remote_objects[i]; | 97 | remote_object_t* obj = remote_objects[i]; |
| 104 | if (obj->object_type == MASTER_TO_ALL_SLAVES || obj->object_type == SLAVE_TO_MASTER) { | 98 | if (obj->object_type == MASTER_TO_ALL_SLAVES || obj->object_type == SLAVE_TO_MASTER) { |
| 105 | triple_buffer_object_t* tb = (triple_buffer_object_t*)obj->buffer; | 99 | triple_buffer_object_t* tb = (triple_buffer_object_t*)obj->buffer; |
| 106 | uint8_t* ptr = (uint8_t*)triple_buffer_read_internal(obj->object_size + LOCAL_OBJECT_EXTRA, tb); | 100 | uint8_t* ptr = (uint8_t*)triple_buffer_read_internal(obj->object_size + LOCAL_OBJECT_EXTRA, tb); |
| 107 | if (ptr) { | 101 | if (ptr) { |
| 108 | ptr[obj->object_size] = i; | 102 | ptr[obj->object_size] = i; |
| 109 | uint8_t dest = obj->object_type == MASTER_TO_ALL_SLAVES ? 0xFF : 0; | 103 | uint8_t dest = obj->object_type == MASTER_TO_ALL_SLAVES ? 0xFF : 0; |
| 110 | router_send_frame(dest, ptr, obj->object_size + 1); | 104 | router_send_frame(dest, ptr, obj->object_size + 1); |
| 111 | } | 105 | } |
| 112 | } | 106 | } else { |
| 113 | else { | 107 | uint8_t* start = obj->buffer; |
| 114 | uint8_t* start = obj->buffer; | ||
| 115 | unsigned int j; | 108 | unsigned int j; |
| 116 | for (j=0;j<NUM_SLAVES;j++) { | 109 | for (j = 0; j < NUM_SLAVES; j++) { |
| 117 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; | 110 | triple_buffer_object_t* tb = (triple_buffer_object_t*)start; |
| 118 | uint8_t* ptr = (uint8_t*)triple_buffer_read_internal(obj->object_size + LOCAL_OBJECT_EXTRA, tb); | 111 | uint8_t* ptr = (uint8_t*)triple_buffer_read_internal(obj->object_size + LOCAL_OBJECT_EXTRA, tb); |
| 119 | if (ptr) { | 112 | if (ptr) { |
| 120 | ptr[obj->object_size] = i; | 113 | ptr[obj->object_size] = i; |
| 121 | uint8_t dest = j + 1; | 114 | uint8_t dest = j + 1; |
| 122 | router_send_frame(dest, ptr, obj->object_size + 1); | 115 | router_send_frame(dest, ptr, obj->object_size + 1); |
| 123 | } | 116 | } |
| 124 | start += LOCAL_OBJECT_SIZE(obj->object_size); | 117 | start += LOCAL_OBJECT_SIZE(obj->object_size); |
