39 /*****************************************************************************/ |
39 /*****************************************************************************/ |
40 |
40 |
41 #include "globals.h" |
41 #include "globals.h" |
42 #include "slave.h" |
42 #include "slave.h" |
43 #include "master.h" |
43 #include "master.h" |
|
44 #include "pdo.h" |
44 #include "sync.h" |
45 #include "sync.h" |
45 |
46 |
46 /*****************************************************************************/ |
47 /*****************************************************************************/ |
47 |
48 |
48 /** |
49 /** |
49 * Constructor. |
50 * Constructor. |
50 */ |
51 */ |
51 |
52 |
52 void ec_sync_init( |
53 void ec_sync_init( |
53 ec_sync_t *sync, /**< EtherCAT sync manager */ |
54 ec_sync_t *sync, /**< EtherCAT sync manager */ |
54 const ec_slave_t *slave, /**< EtherCAT slave */ |
55 ec_slave_t *slave, /**< EtherCAT slave */ |
55 unsigned int index /**< sync manager index */ |
56 unsigned int index /**< sync manager index */ |
56 ) |
57 ) |
57 { |
58 { |
58 sync->slave = slave; |
59 sync->slave = slave; |
59 sync->index = index; |
60 sync->index = index; |
60 |
61 |
61 sync->est_length = 0; |
62 sync->est_length = 0; |
|
63 INIT_LIST_HEAD(&sync->pdos); |
|
64 sync->alt_mapping = 0; |
62 } |
65 } |
63 |
66 |
64 /*****************************************************************************/ |
67 /*****************************************************************************/ |
65 |
68 |
66 /** |
69 /** |
69 |
72 |
70 void ec_sync_clear( |
73 void ec_sync_clear( |
71 ec_sync_t *sync /**< EtherCAT sync manager */ |
74 ec_sync_t *sync /**< EtherCAT sync manager */ |
72 ) |
75 ) |
73 { |
76 { |
|
77 ec_sync_clear_pdos(sync); |
|
78 } |
|
79 |
|
80 /*****************************************************************************/ |
|
81 |
|
82 /** |
|
83 * Calculates the size of a sync manager by evaluating PDO sizes. |
|
84 * \return sync manager size |
|
85 */ |
|
86 |
|
87 uint16_t ec_sync_size( |
|
88 const ec_sync_t *sync /**< sync manager */ |
|
89 ) |
|
90 { |
|
91 const ec_pdo_t *pdo; |
|
92 const ec_pdo_entry_t *pdo_entry; |
|
93 unsigned int bit_size, byte_size; |
|
94 |
|
95 if (sync->length) return sync->length; |
|
96 if (sync->est_length) return sync->est_length; |
|
97 |
|
98 bit_size = 0; |
|
99 list_for_each_entry(pdo, &sync->pdos, list) { |
|
100 list_for_each_entry(pdo_entry, &pdo->entries, list) { |
|
101 bit_size += pdo_entry->bit_length; |
|
102 } |
|
103 } |
|
104 |
|
105 if (bit_size % 8) // round up to full bytes |
|
106 byte_size = bit_size / 8 + 1; |
|
107 else |
|
108 byte_size = bit_size / 8; |
|
109 |
|
110 return byte_size; |
74 } |
111 } |
75 |
112 |
76 /*****************************************************************************/ |
113 /*****************************************************************************/ |
77 |
114 |
78 /** |
115 /** |
83 void ec_sync_config( |
120 void ec_sync_config( |
84 const ec_sync_t *sync, /**< sync manager */ |
121 const ec_sync_t *sync, /**< sync manager */ |
85 uint8_t *data /**> configuration memory */ |
122 uint8_t *data /**> configuration memory */ |
86 ) |
123 ) |
87 { |
124 { |
88 size_t sync_size; |
125 size_t sync_size = ec_sync_size(sync); |
89 |
|
90 sync_size = ec_slave_calc_sync_size(sync->slave, sync); |
|
91 |
126 |
92 if (sync->slave->master->debug_level) { |
127 if (sync->slave->master->debug_level) { |
93 EC_DBG("SM%i: Addr 0x%04X, Size %3i, Ctrl 0x%02X, En %i\n", |
128 EC_DBG("SM%i: Addr 0x%04X, Size %3i, Ctrl 0x%02X, En %i\n", |
94 sync->index, sync->physical_start_address, |
129 sync->index, sync->physical_start_address, |
95 sync_size, sync->control_register, sync->enable); |
130 sync_size, sync->control_register, sync->enable); |
101 EC_WRITE_U8 (data + 5, 0x00); // status byte (read only) |
136 EC_WRITE_U8 (data + 5, 0x00); // status byte (read only) |
102 EC_WRITE_U16(data + 6, sync->enable ? 0x0001 : 0x0000); // enable |
137 EC_WRITE_U16(data + 6, sync->enable ? 0x0001 : 0x0000); // enable |
103 } |
138 } |
104 |
139 |
105 /*****************************************************************************/ |
140 /*****************************************************************************/ |
|
141 |
|
142 /** |
|
143 */ |
|
144 |
|
145 int ec_sync_add_pdo( |
|
146 ec_sync_t *sync, /**< EtherCAT sync manager */ |
|
147 const ec_pdo_t *pdo /**< PDO to map */ |
|
148 ) |
|
149 { |
|
150 ec_pdo_t *mapped_pdo; |
|
151 |
|
152 // PDO already mapped? |
|
153 list_for_each_entry(mapped_pdo, &sync->pdos, list) { |
|
154 if (mapped_pdo->index != pdo->index) continue; |
|
155 EC_ERR("PDO 0x%04X is already mapped!\n", pdo->index); |
|
156 return -1; |
|
157 } |
|
158 |
|
159 if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
|
160 EC_ERR("Failed to allocate memory for PDO mapping.\n"); |
|
161 return -1; |
|
162 } |
|
163 |
|
164 ec_pdo_init(mapped_pdo); |
|
165 if (ec_pdo_copy(mapped_pdo, pdo)) { |
|
166 ec_pdo_clear(mapped_pdo); |
|
167 kfree(mapped_pdo); |
|
168 return -1; |
|
169 } |
|
170 |
|
171 // set appropriate sync manager index |
|
172 mapped_pdo->sync_index = sync->index; |
|
173 |
|
174 list_add_tail(&mapped_pdo->list, &sync->pdos); |
|
175 sync->alt_mapping = 1; |
|
176 return 0; |
|
177 } |
|
178 |
|
179 /*****************************************************************************/ |
|
180 |
|
181 /** |
|
182 */ |
|
183 |
|
184 void ec_sync_clear_pdos( |
|
185 ec_sync_t *sync /**< EtherCAT sync manager */ |
|
186 ) |
|
187 { |
|
188 ec_pdo_t *pdo, *next; |
|
189 |
|
190 // free all mapped PDOs |
|
191 list_for_each_entry_safe(pdo, next, &sync->pdos, list) { |
|
192 list_del(&pdo->list); |
|
193 ec_pdo_clear(pdo); |
|
194 kfree(pdo); |
|
195 } |
|
196 |
|
197 sync->alt_mapping = 1; |
|
198 } |
|
199 |
|
200 /*****************************************************************************/ |