642 | | return true; |
---|
643 | | } |
---|
644 | | |
---|
645 | | bool Ieee1394Service::registerARMHandler(ARMHandler *h) { |
---|
646 | | debugOutput(DEBUG_LEVEL_VERBOSE, "Registering ARM handler (%p) for 0x%016llX, length %u\n", |
---|
647 | | h, h->getStart(), h->getLength()); |
---|
648 | | |
---|
649 | | int err=raw1394_arm_register(m_resetHandle, h->getStart(), |
---|
650 | | h->getLength(), h->getBuffer(), (octlet_t)h, |
---|
651 | | h->getAccessRights(), |
---|
652 | | h->getNotificationOptions(), |
---|
653 | | h->getClientTransactions()); |
---|
654 | | if (err) { |
---|
655 | | debugError("Failed to register ARM handler for 0x%016llX\n", h->getStart()); |
---|
656 | | debugError(" Error: %s\n", strerror(errno)); |
---|
657 | | return false; |
---|
658 | | } |
---|
659 | | |
---|
660 | | m_armHandlers.push_back( h ); |
---|
661 | | |
---|
662 | | return true; |
---|
663 | | } |
---|
664 | | |
---|
665 | | bool Ieee1394Service::unregisterARMHandler( ARMHandler *h ) { |
---|
666 | | debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler (%p) for 0x%016llX\n", |
---|
667 | | h, h->getStart()); |
---|
668 | | |
---|
669 | | for ( arm_handler_vec_t::iterator it = m_armHandlers.begin(); |
---|
670 | | it != m_armHandlers.end(); |
---|
671 | | ++it ) |
---|
672 | | { |
---|
673 | | if((*it) == h) { |
---|
674 | | int err=raw1394_arm_unregister(m_resetHandle, h->getStart()); |
---|
675 | | if (err) { |
---|
676 | | debugError("Failed to unregister ARM handler (%p)\n", h); |
---|
677 | | debugError(" Error: %s\n", strerror(errno)); |
---|
678 | | } else { |
---|
679 | | m_armHandlers.erase(it); |
---|
680 | | return true; |
---|
681 | | } |
---|
682 | | } |
---|
683 | | } |
---|
684 | | debugOutput(DEBUG_LEVEL_VERBOSE, " handler not found!\n"); |
---|
685 | | |
---|
686 | | return false; |
---|
687 | | } |
---|
688 | | /** |
---|
689 | | * @brief Tries to find a free ARM address range |
---|
690 | | * |
---|
691 | | * @param start address to start with |
---|
692 | | * @param length length of the block needed (bytes) |
---|
693 | | * @param step step to use when searching (bytes) |
---|
694 | | * @return The base address that is free, and 0xFFFFFFFFFFFFFFFF when failed |
---|
695 | | */ |
---|
696 | | nodeaddr_t Ieee1394Service::findFreeARMBlock( nodeaddr_t start, size_t length, size_t step ) { |
---|
697 | | debugOutput(DEBUG_LEVEL_VERBOSE, "Finding free ARM block of %d bytes, from 0x%016llX in steps of %d bytes\n", |
---|
698 | | length, start, step); |
---|
699 | | |
---|
700 | | int cnt=0; |
---|
701 | | const int maxcnt=10; |
---|
702 | | int err=1; |
---|
703 | | while(err && cnt++ < maxcnt) { |
---|
704 | | // try to register |
---|
705 | | err=raw1394_arm_register(m_resetHandle, start, length, 0, 0, 0, 0, 0); |
---|
706 | | |
---|
707 | | if (err) { |
---|
708 | | debugOutput(DEBUG_LEVEL_VERBOSE, " -> cannot use 0x%016llX\n", start); |
---|
709 | | debugError(" Error: %s\n", strerror(errno)); |
---|
710 | | start += step; |
---|
711 | | } else { |
---|
712 | | debugOutput(DEBUG_LEVEL_VERBOSE, " -> use 0x%016llX\n", start); |
---|
713 | | err=raw1394_arm_unregister(m_resetHandle, start); |
---|
714 | | if (err) { |
---|
715 | | debugOutput(DEBUG_LEVEL_VERBOSE, " error unregistering test handler\n"); |
---|
716 | | debugError(" Error: %s\n", strerror(errno)); |
---|
717 | | return 0xFFFFFFFFFFFFFFFFLLU; |
---|
718 | | } |
---|
719 | | return start; |
---|
720 | | } |
---|
721 | | } |
---|
722 | | debugOutput(DEBUG_LEVEL_VERBOSE, " Could not find free block in %d tries\n",cnt); |
---|
723 | | return 0xFFFFFFFFFFFFFFFFLLU; |
---|
724 | | } |
---|
725 | | |
---|
726 | | int |
---|
727 | | Ieee1394Service::armHandlerLowLevel(raw1394handle_t handle, |
---|
728 | | unsigned long arm_tag, |
---|
729 | | byte_t request_type, unsigned int requested_length, |
---|
730 | | void *data) |
---|
731 | | { |
---|
732 | | Ieee1394Service* instance |
---|
733 | | = (Ieee1394Service*) raw1394_get_userdata( handle ); |
---|
734 | | instance->armHandler( arm_tag, request_type, requested_length, data ); |
---|
735 | | |
---|
736 | | return 0; |
---|
737 | | } |
---|
738 | | |
---|
739 | | bool |
---|
740 | | Ieee1394Service::armHandler( unsigned long arm_tag, |
---|
741 | | byte_t request_type, unsigned int requested_length, |
---|
742 | | void *data) |
---|
743 | | { |
---|
744 | | for ( arm_handler_vec_t::iterator it = m_armHandlers.begin(); |
---|
745 | | it != m_armHandlers.end(); |
---|
746 | | ++it ) |
---|
747 | | { |
---|
748 | | if((*it) == (ARMHandler *)arm_tag) { |
---|
749 | | struct raw1394_arm_request_response *arm_req_resp; |
---|
750 | | arm_req_resp = (struct raw1394_arm_request_response *) data; |
---|
751 | | raw1394_arm_request_t arm_req=arm_req_resp->request; |
---|
752 | | raw1394_arm_response_t arm_resp=arm_req_resp->response; |
---|
753 | | |
---|
754 | | debugOutput(DEBUG_LEVEL_VERBOSE,"ARM handler for address 0x%016llX called\n", |
---|
755 | | (*it)->getStart()); |
---|
756 | | debugOutput(DEBUG_LEVEL_VERBOSE," request type : 0x%02X\n",request_type); |
---|
757 | | debugOutput(DEBUG_LEVEL_VERBOSE," request length : %04d\n",requested_length); |
---|
758 | | |
---|
759 | | switch(request_type) { |
---|
760 | | case RAW1394_ARM_READ: |
---|
761 | | (*it)->handleRead(arm_req); |
---|
762 | | *arm_resp=*((*it)->getResponse()); |
---|
763 | | break; |
---|
764 | | case RAW1394_ARM_WRITE: |
---|
765 | | (*it)->handleWrite(arm_req); |
---|
766 | | *arm_resp=*((*it)->getResponse()); |
---|
767 | | break; |
---|
768 | | case RAW1394_ARM_LOCK: |
---|
769 | | (*it)->handleLock(arm_req); |
---|
770 | | *arm_resp=*((*it)->getResponse()); |
---|
771 | | break; |
---|
772 | | default: |
---|
773 | | debugWarning("Unknown request type received, ignoring...\n"); |
---|
774 | | } |
---|
775 | | |
---|
776 | | return true; |
---|
777 | | } |
---|
778 | | } |
---|
779 | | |
---|
780 | | debugOutput(DEBUG_LEVEL_VERBOSE,"default ARM handler called\n"); |
---|
781 | | |
---|
782 | | m_default_arm_handler(m_resetHandle, arm_tag, request_type, requested_length, data ); |
---|