42 |
42 |
43 #include "pdo.h" |
43 #include "pdo.h" |
44 |
44 |
45 /*****************************************************************************/ |
45 /*****************************************************************************/ |
46 |
46 |
47 /** |
47 void ec_pdo_clear_entries(ec_pdo_t *); |
48 * PDO constructor. |
48 |
49 */ |
49 /*****************************************************************************/ |
50 |
50 |
51 void ec_pdo_init(ec_pdo_t *pdo /**< EtherCAT PDO */) |
51 /** PDO constructor. |
52 { |
52 */ |
|
53 void ec_pdo_init( |
|
54 ec_pdo_t *pdo /**< EtherCAT PDO */ |
|
55 ) |
|
56 { |
|
57 pdo->sync_index = -1; // not assigned |
53 pdo->name = NULL; |
58 pdo->name = NULL; |
54 INIT_LIST_HEAD(&pdo->entries); |
59 INIT_LIST_HEAD(&pdo->entries); |
55 } |
60 } |
56 |
61 |
57 /*****************************************************************************/ |
62 /*****************************************************************************/ |
58 |
63 |
59 /** |
64 /** Pdo copy constructor. |
60 * PDO destructor. |
65 */ |
61 */ |
66 int ec_pdo_init_copy(ec_pdo_t *pdo, const ec_pdo_t *other_pdo) |
62 |
67 { |
|
68 pdo->dir = other_pdo->dir; |
|
69 pdo->index = other_pdo->index; |
|
70 pdo->sync_index = other_pdo->sync_index; |
|
71 pdo->name = NULL; |
|
72 INIT_LIST_HEAD(&pdo->entries); |
|
73 |
|
74 if (ec_pdo_set_name(pdo, other_pdo->name)) |
|
75 goto out_return; |
|
76 |
|
77 if (ec_pdo_copy_entries(pdo, other_pdo)) |
|
78 goto out_clear; |
|
79 |
|
80 return 0; |
|
81 |
|
82 out_clear: |
|
83 ec_pdo_clear(pdo); |
|
84 out_return: |
|
85 return -1; |
|
86 } |
|
87 |
|
88 /*****************************************************************************/ |
|
89 |
|
90 /** PDO destructor. |
|
91 */ |
63 void ec_pdo_clear(ec_pdo_t *pdo /**< EtherCAT PDO */) |
92 void ec_pdo_clear(ec_pdo_t *pdo /**< EtherCAT PDO */) |
|
93 { |
|
94 if (pdo->name) |
|
95 kfree(pdo->name); |
|
96 |
|
97 ec_pdo_clear_entries(pdo); |
|
98 } |
|
99 |
|
100 /*****************************************************************************/ |
|
101 |
|
102 /** Clear Pdo entry list. |
|
103 */ |
|
104 void ec_pdo_clear_entries(ec_pdo_t *pdo /**< EtherCAT PDO */) |
64 { |
105 { |
65 ec_pdo_entry_t *entry, *next; |
106 ec_pdo_entry_t *entry, *next; |
66 |
107 |
67 // free all PDO entries |
108 // free all PDO entries |
68 list_for_each_entry_safe(entry, next, &pdo->entries, list) { |
109 list_for_each_entry_safe(entry, next, &pdo->entries, list) { |
69 list_del(&entry->list); |
110 list_del(&entry->list); |
|
111 ec_pdo_entry_clear(entry); |
70 kfree(entry); |
112 kfree(entry); |
71 } |
113 } |
72 } |
114 } |
73 |
115 |
74 /*****************************************************************************/ |
116 /*****************************************************************************/ |
75 |
117 |
76 /** |
118 /** Set Pdo name. |
77 * Makes a deep copy of a PDO. |
119 */ |
78 */ |
120 int ec_pdo_set_name( |
79 |
121 ec_pdo_t *pdo, /**< Pdo. */ |
80 int ec_pdo_copy(ec_pdo_t *pdo, const ec_pdo_t *other_pdo) |
122 const char *name /**< New name. */ |
81 { |
123 ) |
82 ec_pdo_entry_t *entry, *other_entry, *next; |
124 { |
83 |
125 unsigned int len; |
84 // make flat copy |
126 |
85 *pdo = *other_pdo; |
127 if (pdo->name) |
86 |
128 kfree(pdo->name); |
87 INIT_LIST_HEAD(&pdo->entries); |
129 |
88 list_for_each_entry(other_entry, &other_pdo->entries, list) { |
130 if (name && (len = strlen(name))) { |
|
131 if (!(pdo->name = (char *) kmalloc(len + 1, GFP_KERNEL))) { |
|
132 EC_ERR("Failed to allocate PDO name.\n"); |
|
133 return -1; |
|
134 } |
|
135 memcpy(pdo->name, name, len + 1); |
|
136 } else { |
|
137 pdo->name = NULL; |
|
138 } |
|
139 |
|
140 return 0; |
|
141 } |
|
142 |
|
143 /*****************************************************************************/ |
|
144 |
|
145 /** Copy Pdo entries from another Pdo. |
|
146 */ |
|
147 int ec_pdo_copy_entries(ec_pdo_t *pdo, const ec_pdo_t *other) |
|
148 { |
|
149 ec_pdo_entry_t *entry, *other_entry; |
|
150 |
|
151 ec_pdo_clear_entries(pdo); |
|
152 |
|
153 list_for_each_entry(other_entry, &other->entries, list) { |
89 if (!(entry = (ec_pdo_entry_t *) |
154 if (!(entry = (ec_pdo_entry_t *) |
90 kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
155 kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
91 EC_ERR("Failed to allocate memory for PDO entry copy.\n"); |
156 EC_ERR("Failed to allocate memory for PDO entry copy.\n"); |
92 goto out_free; |
157 return -1; |
93 } |
158 } |
94 |
159 |
95 *entry = *other_entry; // flat copy is sufficient |
160 if (ec_pdo_entry_init_copy(entry, other_entry)) { |
|
161 kfree(entry); |
|
162 return -1; |
|
163 } |
|
164 |
96 list_add_tail(&entry->list, &pdo->entries); |
165 list_add_tail(&entry->list, &pdo->entries); |
97 } |
166 } |
98 |
167 |
99 return 0; |
168 return 0; |
100 |
169 } |
101 out_free: |
170 |
102 list_for_each_entry_safe(entry, next, &pdo->entries, list) { |
171 /*****************************************************************************/ |
103 list_del(&entry->list); |
172 |
104 kfree(entry); |
173 /** Pdo entry constructor. |
105 } |
174 */ |
106 return -1; |
175 void ec_pdo_entry_init( |
107 } |
176 ec_pdo_entry_t *entry /**< Pdo entry. */ |
108 |
177 ) |
109 /*****************************************************************************/ |
178 { |
|
179 entry->name = NULL; |
|
180 } |
|
181 |
|
182 /*****************************************************************************/ |
|
183 |
|
184 /** Pdo entry copy constructor. |
|
185 */ |
|
186 int ec_pdo_entry_init_copy( |
|
187 ec_pdo_entry_t *entry, /**< Pdo entry. */ |
|
188 const ec_pdo_entry_t *other /**< Pdo entry to copy from. */ |
|
189 ) |
|
190 { |
|
191 entry->index = other->index; |
|
192 entry->subindex = other->subindex; |
|
193 entry->name = NULL; |
|
194 entry->bit_length = other->bit_length; |
|
195 |
|
196 if (ec_pdo_entry_set_name(entry, other->name)) |
|
197 return -1; |
|
198 |
|
199 return 0; |
|
200 } |
|
201 |
|
202 /*****************************************************************************/ |
|
203 |
|
204 /** Pdo entry destructor. |
|
205 */ |
|
206 void ec_pdo_entry_clear(ec_pdo_entry_t *entry /**< Pdo entry. */) |
|
207 { |
|
208 if (entry->name) |
|
209 kfree(entry->name); |
|
210 } |
|
211 |
|
212 /*****************************************************************************/ |
|
213 |
|
214 /** Set Pdo entry name. |
|
215 */ |
|
216 int ec_pdo_entry_set_name( |
|
217 ec_pdo_entry_t *entry, /**< Pdo entry. */ |
|
218 const char *name /**< New name. */ |
|
219 ) |
|
220 { |
|
221 unsigned int len; |
|
222 |
|
223 if (entry->name) |
|
224 kfree(entry->name); |
|
225 |
|
226 if (name && (len = strlen(name))) { |
|
227 if (!(entry->name = (char *) kmalloc(len + 1, GFP_KERNEL))) { |
|
228 EC_ERR("Failed to allocate PDO entry name.\n"); |
|
229 return -1; |
|
230 } |
|
231 memcpy(entry->name, name, len + 1); |
|
232 } else { |
|
233 entry->name = NULL; |
|
234 } |
|
235 |
|
236 return 0; |
|
237 } |
|
238 |
|
239 /*****************************************************************************/ |