310 * is correct (i.e. <= 65536). |
310 * is correct (i.e. <= 65536). |
311 * I decided to go with the other option of using an u16, and |
311 * I decided to go with the other option of using an u16, and |
312 * requiring the user to use addressing starting off at 0! |
312 * requiring the user to use addressing starting off at 0! |
313 */ |
313 */ |
314 /* start_addr = mb_ntoh(u16_v(query_packet[2])) + 1; */ |
314 /* start_addr = mb_ntoh(u16_v(query_packet[2])) + 1; */ |
315 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
315 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
316 mb_ntoh_safe(u16_v(query_packet[4]), &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 */ |
317 |
319 |
318 #ifdef DEBUG |
320 #ifdef DEBUG |
319 printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
321 printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
320 query_packet[0], query_packet[1], start_addr, count); |
322 query_packet[0], query_packet[1], start_addr, count); |
321 #endif |
323 #endif |
374 /* See equivalent comment in handle_read_bits() */ |
376 /* See equivalent comment in handle_read_bits() */ |
375 (*resp_packet_ptr)++; |
377 (*resp_packet_ptr)++; |
376 resp_packet = *resp_packet_ptr; |
378 resp_packet = *resp_packet_ptr; |
377 |
379 |
378 /* See equivalent comment in handle_read_bits() */ |
380 /* See equivalent comment in handle_read_bits() */ |
379 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
381 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
380 mb_ntoh_safe(u16_v(query_packet[4]), &count); |
382 // mb_ntoh_safe(u16_v(query_packet[4]), &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 */ |
381 |
385 |
382 #ifdef DEBUG |
386 #ifdef DEBUG |
383 printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
387 printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
384 query_packet[0], query_packet[1], start_addr, count); |
388 query_packet[0], query_packet[1], start_addr, count); |
385 #endif |
389 #endif |
430 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
434 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
431 |
435 |
432 resp_packet = *resp_packet_ptr; |
436 resp_packet = *resp_packet_ptr; |
433 |
437 |
434 /* See equivalent comment in handle_read_bits() */ |
438 /* See equivalent comment in handle_read_bits() */ |
435 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
439 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
|
440 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
436 |
441 |
437 #ifdef DEBUG |
442 #ifdef DEBUG |
438 printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n", |
443 printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n", |
439 query_packet[0], query_packet[1], start_addr); |
444 query_packet[0], query_packet[1], start_addr); |
440 #endif |
445 #endif |
475 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
480 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
476 |
481 |
477 resp_packet = *resp_packet_ptr; |
482 resp_packet = *resp_packet_ptr; |
478 |
483 |
479 /* See equivalent comment in handle_read_bits() */ |
484 /* See equivalent comment in handle_read_bits() */ |
480 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
485 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
481 |
486 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
|
487 |
482 #ifdef DEBUG |
488 #ifdef DEBUG |
483 printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n", |
489 printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n", |
484 query_packet[0], query_packet[1], start_addr); |
490 query_packet[0], query_packet[1], start_addr); |
485 #endif |
491 #endif |
486 |
492 |
518 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
524 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
519 |
525 |
520 resp_packet = *resp_packet_ptr; |
526 resp_packet = *resp_packet_ptr; |
521 |
527 |
522 /* See equivalent comment in handle_read_bits() */ |
528 /* See equivalent comment in handle_read_bits() */ |
523 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
529 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
524 mb_ntoh_safe(u16_v(query_packet[4]), &count); |
530 // mb_ntoh_safe(u16_v(query_packet[4]), &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 */ |
525 |
533 |
526 #ifdef DEBUG |
534 #ifdef DEBUG |
527 printf("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
535 printf("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
528 query_packet[0], query_packet[1], start_addr, count); |
536 query_packet[0], query_packet[1], start_addr, count); |
529 #endif |
537 #endif |
564 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
572 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
565 |
573 |
566 resp_packet = *resp_packet_ptr; |
574 resp_packet = *resp_packet_ptr; |
567 |
575 |
568 /* See equivalent comment in handle_read_bits() */ |
576 /* See equivalent comment in handle_read_bits() */ |
569 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
577 // mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
570 mb_ntoh_safe(u16_v(query_packet[4]), &count); |
578 // mb_ntoh_safe(u16_v(query_packet[4]), &count); |
571 |
579 start_addr = query_packet[2] * 256 + query_packet[3]; /* Temporary pointer alignment fix */ |
|
580 count = query_packet[4] * 256 + query_packet[5]; /* Temporary pointer alignment fix */ |
|
581 |
572 if ((count > MAX_WRITE_REGS) || (count < 1) || (count*2 != query_packet[6]) ) |
582 if ((count > MAX_WRITE_REGS) || (count < 1) || (count*2 != query_packet[6]) ) |
573 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
583 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
574 |
584 |
575 /* See equivalent comment in handle_read_bits() */ |
585 /* See equivalent comment in handle_read_bits() */ |
576 if (start_addr + count > 65536) |
586 if (start_addr + count > 65536) |