|
1 /***************************************************************************** |
|
2 * |
|
3 * $Id$ |
|
4 * |
|
5 * Copyright (C) 2006-2009 Florian Pose, Ingenieurgemeinschaft IgH |
|
6 * |
|
7 * This file is part of the IgH EtherCAT Master. |
|
8 * |
|
9 * The IgH EtherCAT Master is free software; you can redistribute it and/or |
|
10 * modify it under the terms of the GNU General Public License version 2, as |
|
11 * published by the Free Software Foundation. |
|
12 * |
|
13 * The IgH EtherCAT Master is distributed in the hope that it will be useful, |
|
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General |
|
16 * Public License for more details. |
|
17 * |
|
18 * You should have received a copy of the GNU General Public License along |
|
19 * with the IgH EtherCAT Master; if not, write to the Free Software |
|
20 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
|
21 * |
|
22 * --- |
|
23 * |
|
24 * The license mentioned above concerns the source code only. Using the |
|
25 * EtherCAT technology and brand is only permitted in compliance with the |
|
26 * industrial property and similar rights of Beckhoff Automation GmbH. |
|
27 * |
|
28 ****************************************************************************/ |
|
29 |
|
30 #include <iomanip> |
|
31 #include <sstream> |
|
32 using namespace std; |
|
33 |
|
34 #include "DataTypeHandler.h" |
|
35 |
|
36 #include "ecrt.h" |
|
37 |
|
38 /*****************************************************************************/ |
|
39 |
|
40 DataTypeHandler::DataTypeHandler() |
|
41 { |
|
42 } |
|
43 |
|
44 /****************************************************************************/ |
|
45 |
|
46 const DataTypeHandler::DataType *DataTypeHandler::findDataType( |
|
47 const string &str |
|
48 ) |
|
49 { |
|
50 const DataType *d; |
|
51 |
|
52 for (d = dataTypes; d->name; d++) |
|
53 if (str == d->name) |
|
54 return d; |
|
55 |
|
56 return NULL; // FIXME exception |
|
57 } |
|
58 |
|
59 /****************************************************************************/ |
|
60 |
|
61 const DataTypeHandler::DataType *DataTypeHandler::findDataType(uint16_t code) |
|
62 { |
|
63 const DataType *d; |
|
64 |
|
65 for (d = dataTypes; d->name; d++) |
|
66 if (code == d->code) |
|
67 return d; |
|
68 |
|
69 return NULL; |
|
70 } |
|
71 |
|
72 /****************************************************************************/ |
|
73 |
|
74 size_t DataTypeHandler::interpretAsType( |
|
75 const DataType *type, |
|
76 const string &source, |
|
77 void *target, |
|
78 size_t targetSize |
|
79 ) |
|
80 { |
|
81 stringstream str; |
|
82 size_t dataSize = type->byteSize; |
|
83 |
|
84 str << source; |
|
85 str >> resetiosflags(ios::basefield); // guess base from prefix |
|
86 str.exceptions(ios::failbit); |
|
87 |
|
88 switch (type->code) { |
|
89 case 0x0002: // int8 |
|
90 { |
|
91 int16_t val; // uint8_t is interpreted as char |
|
92 str >> val; |
|
93 if (val > 127 || val < -128) |
|
94 throw ios::failure("Value out of range"); |
|
95 *(uint8_t *) target = val; |
|
96 break; |
|
97 } |
|
98 case 0x0003: // int16 |
|
99 { |
|
100 int16_t val; |
|
101 str >> val; |
|
102 *(int16_t *) target = cpu_to_le16(val); |
|
103 break; |
|
104 } |
|
105 case 0x0004: // int32 |
|
106 { |
|
107 int32_t val; |
|
108 str >> val; |
|
109 *(int32_t *) target = cpu_to_le32(val); |
|
110 break; |
|
111 } |
|
112 case 0x0005: // uint8 |
|
113 { |
|
114 uint16_t val; // uint8_t is interpreted as char |
|
115 str >> val; |
|
116 if (val > 0xff) |
|
117 throw ios::failure("Value out of range"); |
|
118 *(uint8_t *) target = val; |
|
119 break; |
|
120 } |
|
121 case 0x0006: // uint16 |
|
122 { |
|
123 uint16_t val; |
|
124 str >> val; |
|
125 *(uint16_t *) target = cpu_to_le16(val); |
|
126 break; |
|
127 } |
|
128 case 0x0007: // uint32 |
|
129 { |
|
130 uint32_t val; |
|
131 str >> val; |
|
132 *(uint32_t *) target = cpu_to_le32(val); |
|
133 break; |
|
134 } |
|
135 case 0x0009: // string |
|
136 case 0x000a: // octet_string |
|
137 dataSize = str.str().size(); |
|
138 if (dataSize >= targetSize) { |
|
139 stringstream err; |
|
140 err << "String too large"; |
|
141 throw SizeException(err.str()); |
|
142 } |
|
143 str >> (char *) target; |
|
144 break; |
|
145 |
|
146 default: |
|
147 { |
|
148 stringstream err; |
|
149 err << "Unknown data type 0x" << hex << type->code; |
|
150 throw runtime_error(err.str()); |
|
151 } |
|
152 } |
|
153 |
|
154 return dataSize; |
|
155 } |
|
156 |
|
157 /****************************************************************************/ |
|
158 |
|
159 void DataTypeHandler::outputData( |
|
160 ostream &o, |
|
161 const DataType *type, |
|
162 void *data, |
|
163 size_t dataSize |
|
164 ) |
|
165 { |
|
166 if (type->byteSize && dataSize != type->byteSize) { |
|
167 stringstream err; |
|
168 err << "Data type mismatch. Expected " << type->name |
|
169 << " with " << type->byteSize << " byte, but got " |
|
170 << dataSize << " byte."; |
|
171 throw SizeException(err.str()); |
|
172 } |
|
173 |
|
174 o << setfill('0'); |
|
175 |
|
176 switch (type->code) { |
|
177 case 0x0002: // int8 |
|
178 { |
|
179 int val = (int) *(int8_t *) data; |
|
180 o << "0x" << hex << setw(2) << val |
|
181 << " " << dec << val << endl; |
|
182 } |
|
183 break; |
|
184 case 0x0003: // int16 |
|
185 { |
|
186 int16_t val = le16_to_cpup(data); |
|
187 o << "0x" << hex << setw(4) << val |
|
188 << " " << dec << val << endl; |
|
189 } |
|
190 break; |
|
191 case 0x0004: // int32 |
|
192 { |
|
193 int32_t val = le32_to_cpup(data); |
|
194 o << "0x" << hex << setw(8) << val |
|
195 << " " << dec << val << endl; |
|
196 } |
|
197 break; |
|
198 case 0x0005: // uint8 |
|
199 { |
|
200 unsigned int val = (unsigned int) *(uint8_t *) data; |
|
201 o << "0x" << hex << setw(2) << val |
|
202 << " " << dec << val << endl; |
|
203 } |
|
204 break; |
|
205 case 0x0006: // uint16 |
|
206 { |
|
207 uint16_t val = le16_to_cpup(data); |
|
208 o << "0x" << hex << setw(4) << val |
|
209 << " " << dec << val << endl; |
|
210 } |
|
211 break; |
|
212 case 0x0007: // uint32 |
|
213 { |
|
214 uint32_t val = le32_to_cpup(data); |
|
215 o << "0x" << hex << setw(8) << val |
|
216 << " " << dec << val << endl; |
|
217 } |
|
218 break; |
|
219 case 0x0009: // string |
|
220 o << string((const char *) data, dataSize) << endl; |
|
221 break; |
|
222 case 0x000a: // octet_string |
|
223 o << string((const char *) data, dataSize) << endl; |
|
224 break; |
|
225 default: |
|
226 printRawData(o, (const uint8_t *) data, dataSize); // FIXME |
|
227 break; |
|
228 } |
|
229 } |
|
230 |
|
231 /****************************************************************************/ |
|
232 |
|
233 void DataTypeHandler::printRawData( |
|
234 ostream &o, |
|
235 const uint8_t *data, |
|
236 size_t size |
|
237 ) |
|
238 { |
|
239 o << hex << setfill('0'); |
|
240 while (size--) { |
|
241 o << "0x" << setw(2) << (unsigned int) *data++; |
|
242 if (size) |
|
243 o << " "; |
|
244 } |
|
245 o << endl; |
|
246 } |
|
247 |
|
248 /****************************************************************************/ |
|
249 |
|
250 const DataTypeHandler::DataType DataTypeHandler::dataTypes[] = { |
|
251 {"int8", 0x0002, 1}, |
|
252 {"int16", 0x0003, 2}, |
|
253 {"int32", 0x0004, 4}, |
|
254 {"uint8", 0x0005, 1}, |
|
255 {"uint16", 0x0006, 2}, |
|
256 {"uint32", 0x0007, 4}, |
|
257 {"string", 0x0009, 0}, |
|
258 {"octet_string", 0x000a, 0}, |
|
259 {"raw", 0xffff, 0}, |
|
260 //{"int64", 8}, |
|
261 //{"uint64", 8}, |
|
262 {} |
|
263 }; |
|
264 |
|
265 /*****************************************************************************/ |