162 |
162 |
163 /**************************************************************/ |
163 /**************************************************************/ |
164 /* u16 conversion functions to use on little endian platforms */ |
164 /* u16 conversion functions to use on little endian platforms */ |
165 /**************************************************************/ |
165 /**************************************************************/ |
166 |
166 |
167 static inline u16 mb_hton(u16 w) { |
167 /* NOTE: The input parameter must be the address |
168 register u16 tmp; |
168 * of an u16 passed as a pointer to u8 |
169 tmp = (w & 0x00FF); |
169 * |
170 tmp = ((w & 0xFF00) >> 0x08) | (tmp << 0x08); |
170 * We use u8 *ptr as input parameter and read both (ptr+0) and (ptr+1) |
171 return(tmp); |
171 * instead of using u16 *ptr because we sometimes receive data in packtes |
172 } |
172 * that are not aligned on even addresses, so some compilers recognize that |
173 #define mb_ntoh(a) mb_hton(a) |
173 * the given odd address cannot be used as a pointer to u16 and therefore |
174 |
174 * adjust the pointer by (+1) or (-1), basicaly breacking our code! |
175 static inline void mb_hton_count(u16 *w, int count) { |
175 * So, we revert to u8 pointers... for u16 values. |
176 int i; |
176 */ |
177 for (i = 0; i < count; i++) { |
177 static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) { |
|
178 u16_to_ptr[0] = u16_from_ptr[1]; |
|
179 u16_to_ptr[1] = u16_from_ptr[0]; |
|
180 } |
|
181 #define mb_ntoh(a, b) mb_hton(a, b) |
|
182 |
|
183 |
|
184 static inline void mb_hton_count(u8 *u16_ptr, unsigned count) { |
|
185 unsigned i; |
|
186 for (i = 0; i < count*2; i += 2) { |
178 /* swap the bytes around... |
187 /* swap the bytes around... |
179 * a = a ^ b; |
188 * a = a ^ b; |
180 * b = a ^ b; |
189 * b = a ^ b; |
181 * a = a ^ b; |
190 * a = a ^ b; |
182 */ |
191 */ |
183 ((u8 *)(w+i))[0] ^= ((u8 *)(w+i))[1]; |
192 (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; |
184 ((u8 *)(w+i))[1] ^= ((u8 *)(w+i))[0]; |
193 (u16_ptr+i)[1] ^= (u16_ptr+i)[0]; |
185 ((u8 *)(w+i))[0] ^= ((u8 *)(w+i))[1]; |
194 (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; |
186 } |
195 } |
187 } |
196 } |
188 #define mb_ntoh_count(w, count) mb_hton_count(w, count) |
197 #define mb_ntoh_count(w, count) mb_hton_count(w, count) |
189 |
198 |
190 |
199 |
193 # if __BYTE_ORDER == __BIG_ENDIAN |
202 # if __BYTE_ORDER == __BIG_ENDIAN |
194 /***********************************************************/ |
203 /***********************************************************/ |
195 /* u16 conversion functions to use on big endian platforms */ |
204 /* u16 conversion functions to use on big endian platforms */ |
196 /***********************************************************/ |
205 /***********************************************************/ |
197 |
206 |
198 /* We do not need to swap the bytes around! */ |
207 /* We don't need to swap the bytes around! */ |
199 #define mb_ntoh(val) (val) |
208 static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) { |
200 #define mb_hton(val) (val) |
209 u16_to_ptr[0] = u16_from_ptr[0]; |
|
210 u16_to_ptr[1] = u16_from_ptr[1]; |
|
211 } |
|
212 #define mb_ntoh(a, b) mb_hton(a, b) |
|
213 |
201 #define mb_hton_count(w, count) /* empty ! */ |
214 #define mb_hton_count(w, count) /* empty ! */ |
202 #define mb_ntoh_count(w, count) /* empty ! */ |
215 #define mb_ntoh_count(w, count) /* empty ! */ |
203 |
216 |
204 |
217 |
205 # else |
218 # else |
206 |
|
207 /********************************************************/ |
219 /********************************************************/ |
208 /* u16 conversion functions to use on generic platforms */ |
220 /* u16 conversion functions to use on generic platforms */ |
209 /********************************************************/ |
221 /********************************************************/ |
210 |
222 |
211 /* We don't know the byte order, so we revert to the |
223 |
212 * standard htons() and ntohs() ... |
224 /* We can't determine endiannes at compile time, so we do it at runtime. |
213 */ |
225 * With any luck the compiler will be able to determine the result of the |
214 static inline u16 mb_hton(u16 h_value) |
226 * comparison at compile time and end up discarding the non-used code |
215 {return htons(h_value); /* return h_value; */} |
227 * and the 'if' itself from the final executable. |
216 |
228 */ |
217 static inline u16 mb_ntoh(u16 m_value) |
229 |
218 {return ntohs(m_value); /* return m_value; */} |
230 static union {u16 u16; |
219 |
231 u8 u8[2];} endian_ = 0x0102; |
220 static inline void mb_hton_count(u16 *w, int count) |
232 |
221 {int i; for (i = 0; i < count; i++) {w[i] = mb_hton(w[i]);}} |
233 |
222 |
234 static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) { |
223 static inline void mb_ntoh_count(u16 *w, int count) |
235 if (endian_.u8[0] == 0x01) { |
224 {int i; for (i = 0; i < count; i++) {w[i] = mb_ntoh(w[i]);}} |
236 /* machine is big endian -> no swapping */ |
|
237 u16_to_ptr[0] = u16_from_ptr[0]; |
|
238 u16_to_ptr[1] = u16_from_ptr[1]; |
|
239 } else { |
|
240 /* machine is little endian -> we swap bytes around */ |
|
241 u16_to_ptr[0] = u16_from_ptr[1]; |
|
242 u16_to_ptr[1] = u16_from_ptr[0]; |
|
243 } |
|
244 } |
|
245 #define mb_ntoh(a, b) mb_hton(a, b) |
|
246 |
|
247 |
|
248 static inline void mb_hton_count(u8 *u16_ptr, unsigned count) { |
|
249 unsigned i; |
|
250 |
|
251 if (endian_.u8[0] == 0x01) |
|
252 /* machine is big endian. Nothing to do */ |
|
253 return; |
|
254 |
|
255 /* machine is little endian -> we swap bytes around */ |
|
256 for (i = 0; i < count*2; i += 2) { |
|
257 /* swap the bytes around... |
|
258 * a = a ^ b; |
|
259 * b = a ^ b; |
|
260 * a = a ^ b; |
|
261 */ |
|
262 (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; |
|
263 (u16_ptr+i)[1] ^= (u16_ptr+i)[0]; |
|
264 (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; |
|
265 } |
|
266 } |
|
267 #define mb_ntoh_count(w, count) mb_hton_count(w, count) |
225 |
268 |
226 # endif |
269 # endif |
227 # endif |
270 # endif |
228 #endif /* __BYTE_ORDER */ |
271 #endif /* __BYTE_ORDER */ |
229 |
272 |
230 |
|
231 |
|
232 |
|
233 /* Safe versions of the conversion functions! |
|
234 * |
|
235 * Note that these functions always work, whatever the endiannes |
|
236 * of the machine that executes it! |
|
237 * |
|
238 * It is also safe because the resulting value may be stored |
|
239 * on an odd address even on machines that do not allow directly |
|
240 * accessing u16 bit words on odd addresses. |
|
241 */ |
|
242 static inline int mb_hton_safe(u16 from, u16 *to_ptr) { |
|
243 ((u8 *)to_ptr)[1] = (from & 0x00FF); |
|
244 ((u8 *)to_ptr)[0] = ((from & 0xFF00) >> 0x08); |
|
245 return 0; |
|
246 } |
|
247 |
|
248 #define mb_ntoh_safe(a, b) mb_hton_safe(a, b) |
|
249 |
|
250 |
|
251 /* return Most Significant Byte of value; */ |
|
252 static inline u8 msb(u16 value) |
|
253 {return (value >> 8) & 0xFF;} |
|
254 |
|
255 /* return Least Significant Byte of value; */ |
|
256 static inline u8 lsb(u16 value) |
|
257 {return value & 0xFF;} |
|
258 |
|
259 #define u16_v(char_ptr) (*((u16 *)(&(char_ptr)))) |
|
260 |
273 |
261 |
274 |
262 |
275 |
263 |
276 |
264 |
277 |
309 * in turn require us to check whether the address supplied by the user |
322 * in turn require us to check whether the address supplied by the user |
310 * is correct (i.e. <= 65536). |
323 * is correct (i.e. <= 65536). |
311 * I decided to go with the other option of using an u16, and |
324 * I decided to go with the other option of using an u16, and |
312 * requiring the user to use addressing starting off at 0! |
325 * requiring the user to use addressing starting off at 0! |
313 */ |
326 */ |
314 /* start_addr = mb_ntoh(u16_v(query_packet[2])) + 1; */ |
327 mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
315 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
328 mb_ntoh(&(query_packet[4]), (u8 *)&count); |
316 // mb_ntoh_safe(u16_v(query_packet[4]), &count); |
|
317 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
|
318 count = query_packet[4] * 256 + query_packet[5]; /* Temporary pointer alignment fix */ |
|
319 |
329 |
320 #ifdef DEBUG |
330 #ifdef DEBUG |
321 printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
331 printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
322 query_packet[0], query_packet[1], start_addr, count); |
332 query_packet[0], query_packet[1], start_addr, count); |
323 #endif |
333 #endif |
376 /* See equivalent comment in handle_read_bits() */ |
386 /* See equivalent comment in handle_read_bits() */ |
377 (*resp_packet_ptr)++; |
387 (*resp_packet_ptr)++; |
378 resp_packet = *resp_packet_ptr; |
388 resp_packet = *resp_packet_ptr; |
379 |
389 |
380 /* See equivalent comment in handle_read_bits() */ |
390 /* See equivalent comment in handle_read_bits() */ |
381 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
391 mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
382 // mb_ntoh_safe(u16_v(query_packet[4]), &count); |
392 mb_ntoh(&(query_packet[4]), (u8 *)&count); |
383 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
|
384 count = query_packet[4] * 256 + query_packet[5]; /* Temporary pointer alignment fix */ |
|
385 |
393 |
386 #ifdef DEBUG |
394 #ifdef DEBUG |
387 printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
395 printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
388 query_packet[0], query_packet[1], start_addr, count); |
396 query_packet[0], query_packet[1], start_addr, count); |
389 #endif |
397 #endif |
403 res = read_words_callback(callback_arg, start_addr, count, (u16 *)&(resp_packet[3])); |
411 res = read_words_callback(callback_arg, start_addr, count, (u16 *)&(resp_packet[3])); |
404 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
412 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
405 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
413 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
406 |
414 |
407 /* convert all data from host to network byte order. */ |
415 /* convert all data from host to network byte order. */ |
408 mb_hton_count((u16 *)&(resp_packet[3]), count); |
416 mb_hton_count(&(resp_packet[3]), count); |
409 |
417 |
410 return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */ |
418 return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */ |
411 } |
419 } |
412 |
420 |
413 |
421 |
434 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
442 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
435 |
443 |
436 resp_packet = *resp_packet_ptr; |
444 resp_packet = *resp_packet_ptr; |
437 |
445 |
438 /* See equivalent comment in handle_read_bits() */ |
446 /* See equivalent comment in handle_read_bits() */ |
439 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
447 mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
440 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
|
441 |
448 |
442 #ifdef DEBUG |
449 #ifdef DEBUG |
443 printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n", |
450 printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n", |
444 query_packet[0], query_packet[1], start_addr); |
451 query_packet[0], query_packet[1], start_addr); |
445 #endif |
452 #endif |
480 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
487 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
481 |
488 |
482 resp_packet = *resp_packet_ptr; |
489 resp_packet = *resp_packet_ptr; |
483 |
490 |
484 /* See equivalent comment in handle_read_bits() */ |
491 /* See equivalent comment in handle_read_bits() */ |
485 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
492 mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
486 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
493 |
487 |
|
488 #ifdef DEBUG |
494 #ifdef DEBUG |
489 printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n", |
495 printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n", |
490 query_packet[0], query_packet[1], start_addr); |
496 query_packet[0], query_packet[1], start_addr); |
491 #endif |
497 #endif |
492 |
498 |
500 resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
506 resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
501 resp_packet[4] = query_packet[4]; /* value - hi byte */ |
507 resp_packet[4] = query_packet[4]; /* value - hi byte */ |
502 resp_packet[5] = query_packet[5]; /* value - lo byte */ |
508 resp_packet[5] = query_packet[5]; /* value - lo byte */ |
503 |
509 |
504 /* convert data from network to host byte order */ |
510 /* convert data from network to host byte order */ |
505 mb_ntoh_count((u16 *)&(query_packet[4]), 1); |
511 mb_ntoh_count(&(query_packet[4]), 1); |
506 |
512 |
507 res = (callbacks->write_outwords)(callbacks->arg, start_addr, 1, (u16 *)&(query_packet[4])); |
513 res = (callbacks->write_outwords)(callbacks->arg, start_addr, 1, (u16 *)&(query_packet[4])); |
508 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
514 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
509 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
515 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
510 |
516 |
524 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
530 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
525 |
531 |
526 resp_packet = *resp_packet_ptr; |
532 resp_packet = *resp_packet_ptr; |
527 |
533 |
528 /* See equivalent comment in handle_read_bits() */ |
534 /* See equivalent comment in handle_read_bits() */ |
529 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
535 mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
530 // mb_ntoh_safe(u16_v(query_packet[4]), &count); |
536 mb_ntoh(&(query_packet[4]), (u8 *)&count); |
531 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
|
532 count = query_packet[4] * 256 + query_packet[5]; /* Temporary pointer alignment fix */ |
|
533 |
537 |
534 #ifdef DEBUG |
538 #ifdef DEBUG |
535 printf("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
539 printf("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
536 query_packet[0], query_packet[1], start_addr, count); |
540 query_packet[0], query_packet[1], start_addr, count); |
537 #endif |
541 #endif |
572 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
576 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
573 |
577 |
574 resp_packet = *resp_packet_ptr; |
578 resp_packet = *resp_packet_ptr; |
575 |
579 |
576 /* See equivalent comment in handle_read_bits() */ |
580 /* See equivalent comment in handle_read_bits() */ |
577 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
581 mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
578 // mb_ntoh_safe(u16_v(query_packet[4]), &count); |
582 mb_ntoh(&(query_packet[4]), (u8 *)&count); |
579 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
583 |
580 count = query_packet[4] * 256 + query_packet[5]; /* Temporary pointer alignment fix */ |
|
581 |
|
582 if ((count > MAX_WRITE_REGS) || (count < 1) || (count*2 != query_packet[6]) ) |
584 if ((count > MAX_WRITE_REGS) || (count < 1) || (count*2 != query_packet[6]) ) |
583 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
585 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
584 |
586 |
585 /* See equivalent comment in handle_read_bits() */ |
587 /* See equivalent comment in handle_read_bits() */ |
586 if (start_addr + count > 65536) |
588 if (start_addr + count > 65536) |
593 resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
595 resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
594 resp_packet[4] = query_packet[4]; /* count - hi byte */ |
596 resp_packet[4] = query_packet[4]; /* count - hi byte */ |
595 resp_packet[5] = query_packet[5]; /* count - lo byte */ |
597 resp_packet[5] = query_packet[5]; /* count - lo byte */ |
596 |
598 |
597 /* convert all data from network to host byte order */ |
599 /* convert all data from network to host byte order */ |
598 mb_ntoh_count((u16 *)&(query_packet[7]), count); |
600 mb_ntoh_count(&(query_packet[7]), count); |
599 |
601 |
600 res = (callbacks->write_outwords)(callbacks->arg, start_addr, count, (u16 *)&(query_packet[7])); |
602 res = (callbacks->write_outwords)(callbacks->arg, start_addr, count, (u16 *)&(query_packet[7])); |
601 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
603 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
602 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
604 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
603 |
605 |