etisserant@0: /* etisserant@0: This file is part of CanFestival, a library implementing CanOpen Stack. etisserant@0: etisserant@0: Copyright (C): Edouard TISSERANT and Francis DUPIN etisserant@0: etisserant@0: See COPYING file for copyrights details. etisserant@0: etisserant@0: This library is free software; you can redistribute it and/or etisserant@0: modify it under the terms of the GNU Lesser General Public etisserant@0: License as published by the Free Software Foundation; either etisserant@0: version 2.1 of the License, or (at your option) any later version. etisserant@0: etisserant@0: This library is distributed in the hope that it will be useful, etisserant@0: but WITHOUT ANY WARRANTY; without even the implied warranty of etisserant@0: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU etisserant@0: Lesser General Public License for more details. etisserant@0: etisserant@0: You should have received a copy of the GNU Lesser General Public etisserant@0: License along with this library; if not, write to the Free Software etisserant@0: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA etisserant@0: */ etisserant@0: #include "pdo.h" etisserant@0: #include "objacces.h" etisserant@0: etisserant@0: /****************************************************************************/ etisserant@0: UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req) etisserant@0: { etisserant@0: UNS8 i; etisserant@0: if( d->nodeState == Operational ) { etisserant@0: Message m; etisserant@0: etisserant@0: /* Message copy for sending */ etisserant@0: m.cob_id.w = pdo.cobId & 0x7FF; // Because the cobId is 11 bytes length etisserant@0: if ( req == NOT_A_REQUEST ) { etisserant@0: UNS8 i; etisserant@0: m.rtr = NOT_A_REQUEST; etisserant@0: m.len = pdo.len; etisserant@0: //memcpy(&m.data, &pdo.data, m.len); etisserant@0: // This Memcpy depends on packing structure. Avoid etisserant@0: for (i = 0 ; i < pdo.len ; i++) etisserant@0: m.data[i] = pdo.data[i]; etisserant@0: } etisserant@0: else { etisserant@0: m.rtr = REQUEST; etisserant@0: m.len = 0; etisserant@0: } etisserant@0: etisserant@0: MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w); etisserant@0: MSG_WAR(0x3902, " Nb octets : ", m.len); etisserant@0: for (i = 0 ; i < m.len ; i++) { etisserant@0: MSG_WAR(0x3903," data : ", m.data[i]); etisserant@0: } etisserant@0: etisserant@0: return (*d->canSend)(&m); etisserant@0: } // end if etisserant@0: return 0xFF; etisserant@0: } etisserant@0: etisserant@0: /***************************************************************************/ etisserant@0: UNS8 PDOmGR(CO_Data* d, UNS32 cobId) //PDO Manager etisserant@0: { etisserant@0: UNS8 res; etisserant@0: UNS8 i; etisserant@0: s_PDO pdo; etisserant@0: etisserant@0: MSG_WAR(0x3905, "PDOmGR",0); etisserant@0: etisserant@0: /* if PDO is waiting for transmission, etisserant@0: preparation of the message to send */ etisserant@0: pdo.cobId = cobId; etisserant@0: pdo.len = d->process_var.count; etisserant@0: //memcpy(&(pdo.data), &(process_var.data), pdo.len); etisserant@0: // Ce memcpy devrait être portable etisserant@0: for ( i = 0 ; i < pdo.len ; i++) etisserant@0: pdo.data[i] = d->process_var.data[i]; etisserant@0: etisserant@0: res = sendPDO(d, pdo, NOT_A_REQUEST); etisserant@0: etisserant@0: return res; etisserant@0: } etisserant@0: etisserant@0: /**************************************************************************/ etisserant@0: UNS8 buildPDO(CO_Data* d, UNS16 index) etisserant@0: { // DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. etisserant@0: UNS16 ind; etisserant@0: UNS8 subInd; etisserant@0: etisserant@0: UNS8 * pMappingCount = NULL; // count of mapped objects... etisserant@0: // pointer to the var which is mapped to a pdo etisserant@0: // void * pMappedAppObject = NULL; etisserant@0: // pointer fo the var which holds the mapping parameter of an mapping entry etisserant@0: UNS32 * pMappingParameter = NULL; etisserant@0: etisserant@0: UNS8 Size; etisserant@0: UNS8 dataType; etisserant@0: UNS8 offset; etisserant@0: UNS16 offsetObjdict; etisserant@0: UNS16 offsetObjdictPrm; etisserant@0: UNS32 objDict; etisserant@0: etisserant@0: subInd=(UNS8)0x00; etisserant@0: offset = 0x00; etisserant@0: ind = index - 0x1800; etisserant@0: etisserant@0: MSG_WAR(0x3910,"Prepare PDO to send index :", index); etisserant@0: etisserant@0: /* only operational state allows PDO transmission */ etisserant@0: if( d->nodeState != Operational ) { etisserant@0: MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index); etisserant@0: return 0xFF; etisserant@0: } etisserant@0: offsetObjdictPrm = d->firstIndex->PDO_TRS; etisserant@0: offsetObjdict = d->firstIndex->PDO_TRS_MAP; etisserant@0: etisserant@0: if (offsetObjdictPrm && offsetObjdict) etisserant@0: { etisserant@0: /* get mapped objects number to transmit with this PDO */ etisserant@0: pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject; etisserant@0: MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount); etisserant@0: MSG_WAR(0x3913, " at index : ", 0x1A00 + ind); etisserant@0: while (subInd < *pMappingCount) { // Loop on mapped variables etisserant@0: /* get mapping parameters */ etisserant@0: pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject; etisserant@0: MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind); etisserant@0: MSG_WAR(0x3915, " subIndex : ", subInd + 1); etisserant@0: MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter); etisserant@0: // Get the mapped variable etisserant@0: Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); etisserant@0: objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16), etisserant@0: (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF), etisserant@0: (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); etisserant@0: etisserant@0: if (objDict != OD_SUCCESSFUL) { etisserant@0: MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1); etisserant@0: MSG_WAR(0x2920, " Mapped at index : ", (*pMappingParameter) >> 16); etisserant@0: MSG_WAR(0x2921, " subindex : "getODentry, ((*pMappingParameter) >> 8 ) & 0xFF); etisserant@0: return 0xFF; etisserant@0: } etisserant@0: etisserant@0: offset += Size; etisserant@0: d->process_var.count = offset; etisserant@0: subInd++; etisserant@0: }// end Loop on mapped variables etisserant@0: } etisserant@0: return 0; etisserant@0: } etisserant@0: etisserant@0: /**************************************************************************/ etisserant@0: UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId ) etisserant@0: { etisserant@0: UNS32 * pwCobId; etisserant@0: UNS16 offset; etisserant@0: UNS16 lastIndex; etisserant@0: UNS8 err; etisserant@0: etisserant@0: MSG_WAR(0x3930, "sendPDOrequest ",0); etisserant@0: // Sending the request only if the cobid have been found on the PDO receive etisserant@0: // part dictionary etisserant@0: offset = d->firstIndex->PDO_RCV; etisserant@0: lastIndex = d->lastIndex->PDO_RCV; etisserant@0: if (offset) etisserant@0: while (offset <= lastIndex) { etisserant@0: /*get the CobId*/ etisserant@0: pwCobId = d->objdict[offset].pSubindex[1].pObject; etisserant@0: etisserant@0: if ( *pwCobId == cobId ) { etisserant@0: s_PDO pdo; etisserant@0: pdo.cobId = *pwCobId; etisserant@0: pdo.len = 0; etisserant@0: err = sendPDO(d, pdo, REQUEST); etisserant@0: return err; etisserant@0: } etisserant@0: offset++; etisserant@0: } etisserant@0: MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId); etisserant@0: return 0xFF; etisserant@0: } etisserant@0: etisserant@0: etisserant@0: etisserant@0: /***********************************************************************/ etisserant@0: UNS8 proceedPDO(CO_Data* d, Message *m) etisserant@0: { etisserant@0: UNS8 numPdo; etisserant@0: UNS8 numMap; // Number of the mapped varable etisserant@0: UNS8 i; etisserant@0: UNS8 * pMappingCount = NULL; // count of mapped objects... etisserant@0: // pointer to the var which is mapped to a pdo... etisserant@0: // void * pMappedAppObject = NULL; etisserant@0: // pointer fo the var which holds the mapping parameter of an mapping entry etisserant@0: UNS32 * pMappingParameter = NULL; etisserant@0: UNS8 * pTransmissionType = NULL; // pointer to the transmission type etisserant@0: UNS32 * pwCobId = NULL; etisserant@0: UNS8 Size; etisserant@0: UNS8 dataType; etisserant@0: UNS8 offset; etisserant@0: UNS8 status; etisserant@0: UNS32 objDict; etisserant@0: UNS16 offsetObjdict; etisserant@0: UNS16 lastIndex; etisserant@0: status = state1; etisserant@0: etisserant@0: MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff)); etisserant@0: offset = 0x00; etisserant@0: numPdo = 0; etisserant@0: numMap = 0; etisserant@0: if((*m).rtr == NOT_A_REQUEST ) { // The PDO received is not a request. etisserant@0: offsetObjdict = d->firstIndex->PDO_RCV; etisserant@0: lastIndex = d->lastIndex->PDO_RCV; etisserant@0: etisserant@0: /* study of all the PDO stored in the dictionary */ etisserant@0: if(offsetObjdict) etisserant@0: while (offsetObjdict <= lastIndex) { etisserant@0: etisserant@0: switch( status ) { etisserant@0: etisserant@0: case state1: /* data are stored in process_var array */ etisserant@0: //memcpy(&(process_var.data), &m->data, (*m).len); etisserant@0: // Ce memcpy devrait être portable. etisserant@0: for ( i = 0 ; i < m->len ; i++) etisserant@0: d->process_var.data[i] = m->data[i]; etisserant@0: d->process_var.count = (*m).len; etisserant@0: etisserant@0: status = state2; etisserant@0: break; etisserant@0: etisserant@0: case state2: etisserant@0: /* get CobId of the dictionary correspondant to the received PDO */ etisserant@0: pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; etisserant@0: /* check the CobId coherance */ etisserant@0: //*pwCobId is the cobId read in the dictionary at the state 3 etisserant@0: if ( *pwCobId == (*m).cob_id.w ){ etisserant@0: // The cobId is recognized etisserant@0: status = state4; etisserant@0: MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); etisserant@0: break; etisserant@0: } etisserant@0: else { etisserant@0: // cobId received does not match with those write in the dictionnary etisserant@0: numPdo++; etisserant@0: offsetObjdict++; etisserant@0: status = state2; etisserant@0: break; etisserant@0: } etisserant@0: etisserant@0: case state4: /* get mapped objects number */ etisserant@0: // The cobId of the message received has been found in the dictionnary. etisserant@0: offsetObjdict = d->firstIndex->PDO_RCV_MAP; etisserant@0: lastIndex = d->lastIndex->PDO_RCV_MAP; etisserant@0: pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; etisserant@0: numMap = 0; etisserant@0: while (numMap < *pMappingCount) { etisserant@0: pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; etisserant@0: if (pMappingParameter == NULL) { etisserant@0: MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); etisserant@0: return 0xFF; etisserant@0: } etisserant@0: // Get the addresse of the mapped variable. etisserant@0: // detail of *pMappingParameter : etisserant@0: // The 16 hight bits contains the index, the medium 8 bits contains the subindex, etisserant@0: // and the lower 8 bits contains the size of the mapped variable. etisserant@0: etisserant@0: Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); etisserant@0: etisserant@0: objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), etisserant@0: (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), etisserant@0: (void *)&d->process_var.data[offset], &Size, 0 ); etisserant@0: etisserant@0: if(objDict != OD_SUCCESSFUL) { etisserant@0: MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); etisserant@0: MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); etisserant@0: MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); etisserant@0: return 0xFF; etisserant@0: } etisserant@0: etisserant@0: MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); etisserant@0: MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); etisserant@0: MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); etisserant@0: // MSG_WAR(0x3945, " data : ",*((UNS32 *)pMappedAppObject)); etisserant@0: offset += Size; etisserant@0: numMap++; etisserant@0: } // end loop while on mapped variables etisserant@0: etisserant@0: offset=0x00; etisserant@0: numMap = 0; etisserant@0: return 0; etisserant@0: etisserant@0: }// end switch status etisserant@0: }// end while etisserant@0: }// end if Donnees etisserant@0: etisserant@0: etisserant@0: else if ((*m).rtr == REQUEST ){ etisserant@0: MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w); etisserant@0: status = state1; etisserant@0: offsetObjdict = d->firstIndex->PDO_TRS; etisserant@0: lastIndex = d->lastIndex->PDO_TRS; etisserant@0: if(offsetObjdict) while( offsetObjdict <= lastIndex ){ etisserant@0: /* study of all PDO stored in the objects dictionary */ etisserant@0: etisserant@0: switch( status ){ etisserant@0: etisserant@0: case state1: /* check the CobId */ etisserant@0: /* get CobId of the dictionary which match to the received PDO */ etisserant@0: pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject; etisserant@0: if ( *pwCobId == (*m).cob_id.w ) { etisserant@0: status = state4; etisserant@0: break; etisserant@0: } etisserant@0: else { etisserant@0: numPdo++; etisserant@0: offsetObjdict++; etisserant@0: } etisserant@0: status = state1; etisserant@0: break; etisserant@0: etisserant@0: etisserant@0: case state4: /* check transmission type (after request?) */ etisserant@0: pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; etisserant@0: if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) { etisserant@0: status = state5; etisserant@0: break; etisserant@0: } etisserant@0: else { etisserant@0: // The requested PDO is not to send on request. So, does nothing. etisserant@0: MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); etisserant@0: return 0xFF; etisserant@0: } etisserant@0: etisserant@0: case state5: /* get mapped objects number */ etisserant@0: offsetObjdict = d->firstIndex->PDO_TRS_MAP; etisserant@0: lastIndex = d->lastIndex->PDO_TRS_MAP; etisserant@0: pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; etisserant@0: numMap = 0; etisserant@0: while (numMap < *pMappingCount) { etisserant@0: pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; etisserant@0: // Get the mapped variable etisserant@0: Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); etisserant@0: objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16), etisserant@0: (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF), etisserant@0: (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); etisserant@0: if (objDict != OD_SUCCESSFUL) { etisserant@0: MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1); etisserant@0: MSG_WAR(0x2949, " Mapped at index : ", (*pMappingParameter) >> 16); etisserant@0: MSG_WAR(0x2950, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); etisserant@0: return 0xFF; etisserant@0: } etisserant@0: offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3); etisserant@0: d->process_var.count = offset; etisserant@0: numMap++; etisserant@0: etisserant@0: } // end while etisserant@0: PDOmGR( d, *pwCobId ); // Transmit the PDO etisserant@0: return 0; etisserant@0: etisserant@0: }// end switch status etisserant@0: }// end while etisserant@0: }// end if Requete etisserant@0: etisserant@0: return 0; etisserant@0: } etisserant@0: etisserant@0: etisserant@0: etisserant@0: etisserant@0: /*********************************************************************/ etisserant@0: /* TODO : reimplement this using CallBacks */ etisserant@0: /*********************************************************************/ etisserant@0: /* etisserant@0: UNS8 sendPDOevent( CO_Data* d, void * variable ) etisserant@0: { // DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. etisserant@0: UNS32 objDict = 0; etisserant@0: UNS8 ind, sub_ind; etisserant@0: UNS8 status; etisserant@0: UNS8 offset; etisserant@0: UNS8 * pMappingCount = NULL; etisserant@0: UNS32 * pMappingParameter = NULL; etisserant@0: void * pMappedAppObject = NULL; etisserant@0: UNS8 * pTransmissionType = NULL; // pointer to the transmission type etisserant@0: UNS32 * pwCobId = NULL; etisserant@0: UNS8 * pSize; etisserant@0: UNS8 size; etisserant@0: UNS8 dataType; etisserant@0: UNS16 offsetObjdict; etisserant@0: UNS16 offsetObjdictPrm; etisserant@0: UNS16 lastIndex; etisserant@0: UNS8 numMap; etisserant@0: ind = 0x00; etisserant@0: sub_ind = 1; etisserant@0: offset = 0x00; etisserant@0: pSize = &size; etisserant@0: status = state1; etisserant@0: etisserant@0: // look for the index and subindex where the variable is mapped etisserant@0: // Then, send the pdo which contains the variable. etisserant@0: etisserant@0: MSG_WAR (0x3960, "sendPDOevent", 0); etisserant@0: offsetObjdictPrm = d->firstIndex->PDO_TRS; etisserant@0: etisserant@0: offsetObjdict = d->firstIndex->PDO_TRS_MAP; etisserant@0: lastIndex = d->lastIndex->PDO_TRS_MAP; etisserant@0: etisserant@0: if (offsetObjdictPrm && offsetObjdict) etisserant@0: // Loop on PDO Transmit etisserant@0: while(offsetObjdict <= lastIndex){ etisserant@0: // Check the transmission mode etisserant@0: pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject; etisserant@0: if (*pTransmissionType != TRANS_EVENT) { etisserant@0: ind++; etisserant@0: offsetObjdict++; etisserant@0: offsetObjdictPrm++; etisserant@0: continue; etisserant@0: } etisserant@0: pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject; etisserant@0: numMap = 1; // mapped variable etisserant@0: while (numMap <= *pMappingCount) { etisserant@0: pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject; etisserant@0: // Get the variable etisserant@0: objDict = getODentry( d, etisserant@0: (UNS16)((*pMappingParameter) >> 16), etisserant@0: (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF), etisserant@0: (void * *)&pMappedAppObject, pSize, &dataType, 0 ); etisserant@0: if( objDict != OD_SUCCESSFUL ) { etisserant@0: MSG_WAR(0x2961, "Error in dict. at index : ", etisserant@0: (*pMappingParameter) >> (UNS8)16); etisserant@0: etisserant@0: MSG_WAR(0x2962, " subindex : ", etisserant@0: ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); etisserant@0: return 0xFF; etisserant@0: } etisserant@0: if (pMappedAppObject == variable) { // Variable found ! etisserant@0: MSG_WAR(0x3963, "Variable to send found at index : ", etisserant@0: (*pMappingParameter) >> 16); etisserant@0: MSG_WAR(0x3964, " subIndex : ", etisserant@0: ((*pMappingParameter) >> 8 ) & 0x000000FF); etisserant@0: buildPDO(d, 0x1800 + ind); etisserant@0: // Get the cobId etisserant@0: pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject; etisserant@0: PDOmGR( d, *pwCobId ); // Send the PDO etisserant@0: return 0; etisserant@0: } etisserant@0: numMap++; etisserant@0: } // End loop on mapped variable etisserant@0: ind++; etisserant@0: offsetObjdict++; etisserant@0: offsetObjdictPrm++; etisserant@0: } // End loop while on PDO etisserant@0: etisserant@0: MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0); etisserant@0: return 0xFF; etisserant@0: etisserant@0: } etisserant@0: */