ethercatcoe.c
Go to the documentation of this file.
00001 /*
00002  * Simple Open EtherCAT Master Library 
00003  *
00004  * File    : ethercatcoe.c
00005  * Version : 1.3.0
00006  * Date    : 24-02-2013
00007  * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f.
00008  * Copyright (C) 2005-2013 Arthur Ketels
00009  * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 
00010  *
00011  * SOEM is free software; you can redistribute it and/or modify it under
00012  * the terms of the GNU General Public License version 2 as published by the Free
00013  * Software Foundation.
00014  *
00015  * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY
00016  * WARRANTY; without even the implied warranty of MERCHANTABILITY or
00017  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
00018  * for more details.
00019  *
00020  * As a special exception, if other files instantiate templates or use macros
00021  * or inline functions from this file, or you compile this file and link it
00022  * with other works to produce a work based on this file, this file does not
00023  * by itself cause the resulting work to be covered by the GNU General Public
00024  * License. However the source code for this file must still be made available
00025  * in accordance with section (3) of the GNU General Public License.
00026  *
00027  * This exception does not invalidate any other reasons why a work based on
00028  * this file might be covered by the GNU General Public License.
00029  *
00030  * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual
00031  * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for
00032  * the sole purpose of creating, using and/or selling or otherwise distributing
00033  * an EtherCAT network master provided that an EtherCAT Master License is obtained
00034  * from Beckhoff Automation GmbH.
00035  *
00036  * In case you did not receive a copy of the EtherCAT Master License along with
00037  * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany
00038  * (www.beckhoff.com).
00039  */
00040 
00048 #include <stdio.h>
00049 #include <string.h>
00050 #include "ethercat_soem/osal.h"
00051 #include "ethercat_soem/oshw.h"
00052 #include "ethercat_soem/ethercattype.h"
00053 #include "ethercat_soem/ethercatbase.h"
00054 #include "ethercat_soem/ethercatmain.h"
00055 #include "ethercat_soem/ethercatcoe.h"
00056 
00058 PACKED_BEGIN
00059 typedef struct PACKED
00060 {
00061    ec_mbxheadert   MbxHeader;
00062    uint16          CANOpen;
00063    uint8           Command;
00064    uint16          Index;
00065    uint8           SubIndex;
00066    union
00067    {
00068       uint8   bdata[0x200]; /* variants for easy data access */
00069       uint16  wdata[0x100];
00070       uint32  ldata[0x80];
00071    };
00072 } ec_SDOt;
00073 PACKED_END
00074 
00076 PACKED_BEGIN
00077 typedef struct PACKED 
00078 {
00079    ec_mbxheadert   MbxHeader;
00080    uint16          CANOpen;
00081    uint8           Opcode;
00082    uint8           Reserved;
00083    uint16          Fragments;
00084    union
00085    {
00086       uint8   bdata[0x200]; /* variants for easy data access */
00087       uint16  wdata[0x100];
00088       uint32  ldata[0x80];
00089    };
00090 } ec_SDOservicet;
00091 PACKED_END
00092 
00101 void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
00102 {
00103    ec_errort Ec;
00104 
00105    Ec.Time = osal_current_time();
00106    Ec.Slave = Slave;
00107    Ec.Index = Index;
00108    Ec.SubIdx = SubIdx;
00109    *(context->ecaterror) = TRUE;
00110    Ec.Etype = EC_ERR_TYPE_SDO_ERROR;
00111    Ec.AbortCode = AbortCode;
00112    ecx_pusherror(context, &Ec);
00113 }
00114 
00123 static void ecx_SDOinfoerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
00124 {
00125    ec_errort Ec;
00126 
00127    Ec.Slave = Slave;
00128    Ec.Index = Index;
00129    Ec.SubIdx = SubIdx;
00130    *(context->ecaterror) = TRUE;
00131    Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR;
00132    Ec.AbortCode = AbortCode;
00133    ecx_pusherror(context, &Ec);
00134 }
00135 
00153 int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex,
00154                boolean CA, int *psize, void *p, int timeout)
00155 {
00156    ec_SDOt *SDOp, *aSDOp;
00157    uint16 bytesize, Framedatasize;
00158    int wkc;
00159    int32 SDOlen;
00160    uint8 *bp;
00161    uint8 *hp;
00162    ec_mbxbuft MbxIn, MbxOut;
00163    uint8 cnt, toggle;
00164    boolean NotLast;
00165 
00166    ec_clearmbx(&MbxIn);
00167    /* Empty slave out mailbox if something is in. Timout set to 0 */
00168    wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
00169    ec_clearmbx(&MbxOut);
00170    aSDOp = (ec_SDOt *)&MbxIn;
00171    SDOp = (ec_SDOt *)&MbxOut;
00172    SDOp->MbxHeader.length = htoes(0x000a);
00173    SDOp->MbxHeader.address = htoes(0x0000);
00174    SDOp->MbxHeader.priority = 0x00;
00175    /* get new mailbox count value, used as session handle */
00176    cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00177    context->slavelist[slave].mbx_cnt = cnt;
00178    SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
00179    SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */
00180    if (CA)
00181    {
00182       SDOp->Command = ECT_SDO_UP_REQ_CA; /* upload request complete access */
00183    }
00184    else
00185    {
00186       SDOp->Command = ECT_SDO_UP_REQ; /* upload request normal */
00187    }
00188    SDOp->Index = htoes(index);
00189    if (CA && (subindex > 1))
00190    {
00191       subindex = 1;
00192    }
00193    SDOp->SubIndex = subindex;
00194    SDOp->ldata[0] = 0;
00195    /* send CoE request to slave */
00196    wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00197    if (wkc > 0) /* succeeded to place mailbox in slave ? */
00198    {
00199       /* clean mailboxbuffer */
00200       ec_clearmbx(&MbxIn);
00201       /* read slave response */
00202       wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00203       if (wkc > 0) /* succeeded to read slave response ? */
00204       {
00205          /* slave response should be CoE, SDO response and the correct index */
00206          if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00207              ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00208               (aSDOp->Index == SDOp->Index))
00209          {
00210             if ((aSDOp->Command & 0x02) > 0)
00211             { 
00212                /* expedited frame response */
00213                bytesize = 4 - ((aSDOp->Command >> 2) & 0x03);
00214                if (*psize >= bytesize) /* parameter buffer big enough ? */
00215                {
00216                   /* copy parameter in parameter buffer */
00217                   memcpy(p, &aSDOp->ldata[0], bytesize);
00218                   /* return the real parameter size */
00219                   *psize = bytesize;
00220                }
00221                else
00222                {
00223                   wkc = 0;
00224                   ecx_packeterror(context, slave, index, subindex, 3); /*  data container too small for type */
00225                }
00226             }
00227             else
00228             { /* normal frame response */
00229                SDOlen = etohl(aSDOp->ldata[0]);
00230                /* Does parameter fit in parameter buffer ? */
00231                if (SDOlen <= *psize)
00232                {
00233                   bp = p; 
00234                   hp = p;
00235                   /* calculate mailbox transfer size */
00236                   Framedatasize = (etohs(aSDOp->MbxHeader.length) - 10);
00237                   if (Framedatasize < SDOlen) /* transfer in segments? */
00238                   {
00239                      /* copy parameter data in parameter buffer */
00240                      memcpy(hp, &aSDOp->ldata[1], Framedatasize);
00241                      /* increment buffer pointer */
00242                      hp += Framedatasize;
00243                      *psize = Framedatasize;
00244                      NotLast = TRUE;
00245                      toggle= 0x00;
00246                      while (NotLast) /* segmented transfer */
00247                      {
00248                         SDOp = (ec_SDOt *)&MbxOut;
00249                         SDOp->MbxHeader.length = htoes(0x000a);
00250                         SDOp->MbxHeader.address = htoes(0x0000);
00251                         SDOp->MbxHeader.priority = 0x00;
00252                         cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00253                         context->slavelist[slave].mbx_cnt = cnt;
00254                         SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
00255                         SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */
00256                         SDOp->Command = ECT_SDO_SEG_UP_REQ + toggle; /* segment upload request */
00257                         SDOp->Index = htoes(index);
00258                         SDOp->SubIndex = subindex;
00259                         SDOp->ldata[0] = 0;
00260                         /* send segmented upload request to slave */
00261                         wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00262                         /* is mailbox transfered to slave ? */
00263                         if (wkc > 0)
00264                         {
00265                             ec_clearmbx(&MbxIn);
00266                            /* read slave response */
00267                            wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00268                            /* has slave responded ? */
00269                            if (wkc > 0)
00270                            {
00271                               /* slave response should be CoE, SDO response */
00272                               if ((((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00273                                    ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && 
00274                                    ((aSDOp->Command & 0xe0) == 0x00)))
00275                                         {
00276                                  /* calculate mailbox transfer size */
00277                                  Framedatasize = etohs(aSDOp->MbxHeader.length) - 3;
00278                                  if ((aSDOp->Command & 0x01) > 0)
00279                                  { /* last segment */
00280                                     NotLast = FALSE;
00281                                     if (Framedatasize == 7)
00282                                        /* substract unused bytes from frame */
00283                                        Framedatasize = Framedatasize - ((aSDOp->Command & 0x0e) >> 1);
00284                                     /* copy to parameter buffer */
00285                                     memcpy(hp, &(aSDOp->Index), Framedatasize);
00286                                  }
00287                                  else /* segments follow */
00288                                  {
00289                                     /* copy to parameter buffer */
00290                                      memcpy(hp, &(aSDOp->Index), Framedatasize);
00291                                     /* increment buffer pointer */
00292                                     hp += Framedatasize;
00293                                  }
00294                                  /* update parametersize */
00295                                  *psize += Framedatasize;
00296                               }
00297                               /* unexpected frame returned from slave */
00298                               else
00299                               {
00300                                  NotLast = FALSE;
00301                                  if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
00302                                     ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0]));
00303                                  else
00304                                     ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */
00305                                  wkc = 0;
00306                               }
00307                            }
00308                         }
00309                         toggle = toggle ^ 0x10; /* toggle bit for segment request */
00310                      }
00311                   }
00312                   /* non segmented transfer */
00313                   else
00314                   {
00315                      /* copy to parameter buffer */
00316                      memcpy(bp, &aSDOp->ldata[1], SDOlen);
00317                      *psize = SDOlen;
00318                   }
00319                }
00320                /* parameter buffer too small */
00321                else
00322                {
00323                   wkc = 0;
00324                   ecx_packeterror(context, slave, index, subindex, 3); /*  data container too small for type */
00325                }
00326             }
00327          }
00328          /* other slave response */
00329          else
00330          {
00331             if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
00332             {
00333                ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0]));
00334             }
00335             else
00336             {
00337                ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */
00338             }
00339             wkc = 0;
00340          }
00341       }
00342    }
00343    return wkc;
00344 }
00345 
00363 int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex,
00364                 boolean CA, int psize, void *p, int Timeout)
00365 {
00366    ec_SDOt *SDOp, *aSDOp;
00367    int wkc, maxdata;
00368    ec_mbxbuft MbxIn, MbxOut;
00369    uint8 cnt, toggle;
00370    uint16 framedatasize;
00371    boolean  NotLast;
00372    uint8 *hp;
00373 
00374    ec_clearmbx(&MbxIn);
00375    /* Empty slave out mailbox if something is in. Timout set to 0 */
00376    wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0);
00377    ec_clearmbx(&MbxOut);
00378    aSDOp = (ec_SDOt *)&MbxIn;
00379    SDOp = (ec_SDOt *)&MbxOut;
00380    maxdata = context->slavelist[Slave].mbx_l - 0x10; /* data section=mailbox size - 6 mbx - 2 CoE - 8 sdo req */
00381    /* if small data use expedited transfer */
00382    if ((psize <= 4) && !CA)
00383    {
00384       SDOp->MbxHeader.length = htoes(0x000a);
00385       SDOp->MbxHeader.address = htoes(0x0000);
00386       SDOp->MbxHeader.priority = 0x00;
00387       /* get new mailbox counter, used for session handle */
00388       cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
00389       context->slavelist[Slave].mbx_cnt = cnt;
00390       SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
00391       SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */
00392       SDOp->Command = ECT_SDO_DOWN_EXP | (((4 - psize) << 2) & 0x0c); /* expedited SDO download transfer */
00393       SDOp->Index = htoes(Index);
00394       SDOp->SubIndex = SubIndex;
00395       hp = p;
00396       /* copy parameter data to mailbox */
00397       memcpy(&SDOp->ldata[0], hp, psize);
00398       /* send mailbox SDO download request to slave */
00399       wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00400       if (wkc > 0)
00401       {
00402          ec_clearmbx(&MbxIn);
00403          /* read slave response */
00404          wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
00405          if (wkc > 0)
00406          {
00407             /* response should be CoE, SDO response, correct index and subindex */
00408             if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 
00409                 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00410                  (aSDOp->Index == SDOp->Index) &&
00411                  (aSDOp->SubIndex == SDOp->SubIndex))
00412             {
00413                  /* all OK */
00414             }
00415             /* unexpected response from slave */
00416             else
00417             {
00418                if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */
00419                {
00420                   ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
00421                }
00422                else
00423                {
00424                   ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */
00425                }
00426                wkc = 0;
00427             }
00428          }
00429       }
00430    }
00431    else
00432    {
00433       framedatasize = psize;
00434       NotLast = FALSE;
00435       if (framedatasize > maxdata)
00436       {
00437          framedatasize = maxdata;  /*  segmented transfer needed  */
00438          NotLast = TRUE;
00439       }
00440       SDOp->MbxHeader.length = htoes(0x0a + framedatasize);
00441       SDOp->MbxHeader.address = htoes(0x0000);
00442       SDOp->MbxHeader.priority = 0x00;
00443       /* get new mailbox counter, used for session handle */
00444       cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
00445       context->slavelist[Slave].mbx_cnt = cnt;
00446       SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
00447       SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */
00448       if (CA)
00449       {
00450          SDOp->Command = ECT_SDO_DOWN_INIT_CA; /* Complete Access, normal SDO init download transfer */
00451       }
00452       else
00453       {
00454          SDOp->Command = ECT_SDO_DOWN_INIT; /* normal SDO init download transfer */
00455       }
00456       SDOp->Index = htoes(Index);
00457       SDOp->SubIndex = SubIndex;
00458       if (CA && (SubIndex > 1))
00459       {
00460          SDOp->SubIndex = 1;
00461       }
00462       SDOp->ldata[0] = htoel(psize);
00463       hp = p;
00464       /* copy parameter data to mailbox */
00465       memcpy(&SDOp->ldata[1], hp, framedatasize);
00466       hp += framedatasize;
00467       psize -= framedatasize;
00468       /* send mailbox SDO download request to slave */
00469       wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00470       if (wkc > 0)
00471       {
00472          ec_clearmbx(&MbxIn);
00473          /* read slave response */
00474          wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
00475          if (wkc > 0)
00476          {
00477             /* response should be CoE, SDO response, correct index and subindex */
00478             if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && 
00479                 ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00480                  (aSDOp->Index == SDOp->Index) &&
00481                  (aSDOp->SubIndex == SDOp->SubIndex))
00482             {
00483                /* all ok */
00484                maxdata += 7;
00485                toggle = 0;
00486                /* repeat while segments left */
00487                while (NotLast)
00488                {
00489                   SDOp = (ec_SDOt *)&MbxOut;
00490                   framedatasize = psize;
00491                   NotLast = FALSE;
00492                   SDOp->Command = 0x01; /* last segment */
00493                   if (framedatasize > maxdata)
00494                   {
00495                      framedatasize = maxdata;  /*  more segments needed  */
00496                      NotLast = TRUE;
00497                      SDOp->Command = 0x00; /* segments follow */
00498                   }
00499                   if (!NotLast && (framedatasize < 7))
00500                   {
00501                      SDOp->MbxHeader.length = htoes(0x0a); /* minimum size */
00502                      SDOp->Command = 0x01 + ((7 - framedatasize) << 1); /* last segment reduced octets */
00503                   }
00504                   else
00505                   {
00506                      SDOp->MbxHeader.length = htoes(framedatasize + 3); /* data + 2 CoE + 1 SDO */
00507                   }
00508                   SDOp->MbxHeader.address = htoes(0x0000);
00509                   SDOp->MbxHeader.priority = 0x00;
00510                   /* get new mailbox counter value */
00511                   cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
00512                   context->slavelist[Slave].mbx_cnt = cnt;
00513                   SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
00514                   SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */
00515                   SDOp->Command = SDOp->Command + toggle; /* add toggle bit to command byte */
00516                   /* copy parameter data to mailbox */
00517                   memcpy(&SDOp->Index, hp, framedatasize);
00518                   /* update parameter buffer pointer */
00519                   hp += framedatasize;
00520                   psize -= framedatasize;
00521                   /* send SDO download request */
00522                   wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00523                   if (wkc > 0)
00524                   {
00525                      ec_clearmbx(&MbxIn);
00526                      /* read slave response */
00527                      wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, Timeout);
00528                      if (wkc > 0)
00529                      {
00530                         if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00531                             ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
00532                             ((aSDOp->Command & 0xe0) == 0x20))
00533                         {
00534                                    /* all OK, nothing to do */
00535                         }
00536                         else
00537                         {
00538                            if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */
00539                            {
00540                               ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
00541                            }
00542                            else
00543                            {
00544                               ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */
00545                            }
00546                            wkc = 0;
00547                            NotLast = FALSE;
00548                         }
00549                      }
00550                   }
00551                   toggle = toggle ^ 0x10; /* toggle bit for segment request */
00552                }
00553             }
00554             /* unexpected response from slave */
00555             else
00556             {
00557                if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */
00558                {
00559                   ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
00560                }
00561                else
00562                {
00563                   ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */
00564                }
00565                wkc = 0;
00566             }
00567          }
00568       }
00569    }
00570 
00571    return wkc;
00572 }
00573 
00585 int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber, int psize, void *p)
00586 {
00587    ec_SDOt *SDOp;
00588    int wkc, maxdata;
00589    ec_mbxbuft MbxIn, MbxOut;
00590    uint8 cnt;
00591    uint16 framedatasize;
00592 
00593    ec_clearmbx(&MbxIn);
00594    /* Empty slave out mailbox if something is in. Timout set to 0 */
00595    wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0);
00596    ec_clearmbx(&MbxOut);
00597    SDOp = (ec_SDOt *)&MbxOut;
00598    maxdata = context->slavelist[Slave].mbx_l - 0x08; /* data section=mailbox size - 6 mbx - 2 CoE */
00599    framedatasize = psize;
00600    if (framedatasize > maxdata)
00601    {
00602       framedatasize = maxdata;  /*  limit transfer */
00603    }
00604    SDOp->MbxHeader.length = htoes(0x02 + framedatasize);
00605    SDOp->MbxHeader.address = htoes(0x0000);
00606    SDOp->MbxHeader.priority = 0x00;
00607    /* get new mailbox counter, used for session handle */
00608    cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
00609    context->slavelist[Slave].mbx_cnt = cnt;
00610    SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
00611    SDOp->CANOpen = htoes((RxPDOnumber & 0x01ff) + (ECT_COES_RXPDO << 12)); /* number 9bits service upper 4 bits */
00612    /* copy PDO data to mailbox */
00613    memcpy(&SDOp->Command, p, framedatasize);
00614    /* send mailbox RxPDO request to slave */
00615    wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00616 
00617    return wkc;
00618 }
00619 
00632 int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout)
00633 {
00634    ec_SDOt *SDOp, *aSDOp;
00635    int wkc;
00636    ec_mbxbuft MbxIn, MbxOut;
00637    uint8 cnt;
00638    uint16 framedatasize;
00639 
00640    ec_clearmbx(&MbxIn);
00641    /* Empty slave out mailbox if something is in. Timout set to 0 */
00642    wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
00643    ec_clearmbx(&MbxOut);
00644    aSDOp = (ec_SDOt *)&MbxIn;
00645    SDOp = (ec_SDOt *)&MbxOut;
00646    SDOp->MbxHeader.length = htoes(0x02);
00647    SDOp->MbxHeader.address = htoes(0x0000);
00648    SDOp->MbxHeader.priority = 0x00;
00649    /* get new mailbox counter, used for session handle */
00650    cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00651    context->slavelist[slave].mbx_cnt = cnt;
00652    SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
00653    SDOp->CANOpen = htoes((TxPDOnumber & 0x01ff) + (ECT_COES_TXPDO_RR << 12)); /* number 9bits service upper 4 bits */
00654    wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00655    if (wkc > 0)
00656    {
00657       /* clean mailboxbuffer */
00658       ec_clearmbx(&MbxIn);
00659       /* read slave response */
00660       wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00661       if (wkc > 0) /* succeeded to read slave response ? */
00662       {
00663          /* slave response should be CoE, TxPDO */
00664          if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
00665              ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_TXPDO))
00666          {
00667             /* TxPDO response */
00668             framedatasize = (aSDOp->MbxHeader.length - 2);
00669             if (*psize >= framedatasize) /* parameter buffer big enough ? */
00670             {
00671                /* copy parameter in parameter buffer */
00672                memcpy(p, &aSDOp->Command, framedatasize);
00673                /* return the real parameter size */
00674                *psize = framedatasize;
00675             }
00676             /* parameter buffer too small */
00677             else
00678             {
00679                wkc = 0;
00680                ecx_packeterror(context, slave, 0, 0, 3); /*  data container too small for type */
00681             }
00682          }   
00683          /* other slave response */
00684          else
00685          {
00686             if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
00687             {
00688                ecx_SDOerror(context, slave, 0, 0, etohl(aSDOp->ldata[0]));
00689             }
00690             else
00691             {
00692                ecx_packeterror(context, slave, 0, 0, 1); /* Unexpected frame returned */
00693             }
00694             wkc = 0;
00695          }
00696       }         
00697    }
00698 
00699    return wkc;
00700 }
00701 
00708 int ecx_readPDOassign(ecx_contextt *context, uint16 Slave, uint16 PDOassign)
00709 {
00710    uint16 idxloop, nidx, subidxloop, rdat, idx, subidx;
00711    uint8 subcnt;
00712    int wkc, bsize = 0, rdl;
00713    int32 rdat2;
00714 
00715    rdl = sizeof(rdat); rdat = 0;
00716    /* read PDO assign subindex 0 ( = number of PDO's) */
00717    wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
00718    rdat = etohs(rdat);
00719    /* positive result from slave ? */
00720    if ((wkc > 0) && (rdat > 0))
00721    {
00722       /* number of available sub indexes */
00723       nidx = rdat;
00724       bsize = 0;
00725       /* read all PDO's */
00726       for (idxloop = 1; idxloop <= nidx; idxloop++)
00727       {
00728          rdl = sizeof(rdat); rdat = 0;
00729          /* read PDO assign */
00730          wkc = ecx_SDOread(context, Slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
00731          /* result is index of PDO */
00732          idx = etohl(rdat);
00733          if (idx > 0)
00734          {
00735             rdl = sizeof(subcnt); subcnt = 0;
00736             /* read number of subindexes of PDO */
00737             wkc = ecx_SDOread(context, Slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM);
00738             subidx = subcnt;
00739             /* for each subindex */
00740             for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
00741             {
00742                rdl = sizeof(rdat2); rdat2 = 0;
00743                /* read SDO that is mapped in PDO */
00744                wkc = ecx_SDOread(context, Slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM);
00745                rdat2 = etohl(rdat2);
00746                /* extract bitlength of SDO */
00747                if (LO_BYTE(rdat2) < 0xff)
00748                {
00749                   bsize += LO_BYTE(rdat2);
00750                }
00751                else
00752                {
00753                   rdl = sizeof(rdat); rdat = htoes(0xff);
00754                   /* read Object Entry in Object database */
00755 //                  wkc = ec_readOEsingle(idx, (uint8)SubCount, pODlist, pOElist);
00756                   bsize += etohs(rdat);                  
00757                }
00758             }
00759          }
00760       }
00761    }
00762    /* return total found bitlength (PDO) */
00763    return bsize;
00764 }
00765 
00772 int ecx_readPDOassignCA(ecx_contextt *context, uint16 Slave, uint16 PDOassign)
00773 {
00774    uint16 idxloop, nidx, subidxloop, idx, subidx;
00775    int wkc, bsize = 0, rdl;
00776 
00777    /* find maximum size of PDOassign buffer */
00778    rdl = sizeof(ec_PDOassignt); 
00779    context->PDOassign->n=0;
00780    /* read rxPDOassign in CA mode, all subindexes are read in one struct */
00781    wkc = ecx_SDOread(context, Slave, PDOassign, 0x00, TRUE, &rdl, context->PDOassign, EC_TIMEOUTRXM);
00782    /* positive result from slave ? */
00783    if ((wkc > 0) && (context->PDOassign->n > 0))
00784    {
00785       nidx = context->PDOassign->n;
00786       bsize = 0;
00787       /* for each PDO do */
00788       for (idxloop = 1; idxloop <= nidx; idxloop++)
00789       {
00790          /* get index from PDOassign struct */
00791          idx = etohs(context->PDOassign->index[idxloop - 1]);
00792          if (idx > 0)
00793          {
00794             rdl = sizeof(ec_PDOdesct); context->PDOdesc->n = 0;
00795             /* read SDO's that are mapped in PDO, CA mode */
00796             wkc = ecx_SDOread(context, Slave,idx, 0x00, TRUE, &rdl, context->PDOdesc, EC_TIMEOUTRXM);
00797             subidx = context->PDOdesc->n;
00798             /* extract all bitlengths of SDO's */
00799             for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
00800             {
00801                bsize += LO_BYTE(etohl(context->PDOdesc->PDO[subidxloop -1]));
00802             }
00803          }
00804       }
00805    }
00806 
00807    /* return total found bitlength (PDO) */
00808    return bsize;
00809 }
00810 
00839 int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize)
00840 {
00841    int wkc, rdl;
00842    int retVal = 0;
00843    uint8 nSM, iSM, tSM;
00844    int Tsize;
00845    uint8 SMt_bug_add;
00846    
00847    *Isize = 0;
00848    *Osize = 0;
00849    SMt_bug_add = 0;
00850    rdl = sizeof(nSM); nSM = 0;
00851    /* read SyncManager Communication Type object count */
00852    wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM);
00853    /* positive result from slave ? */
00854    if ((wkc > 0) && (nSM > 2))
00855    {
00856       /* make nSM equal to number of defined SM */
00857       nSM--;
00858       /* limit to maximum number of SM defined, if true the slave can't be configured */
00859       if (nSM > EC_MAXSM)
00860          nSM = EC_MAXSM;
00861       /* iterate for every SM type defined */
00862       for (iSM = 2 ; iSM <= nSM ; iSM++)
00863       {
00864          rdl = sizeof(tSM); tSM = 0;
00865          /* read SyncManager Communication Type */
00866          wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM);
00867          if (wkc > 0)
00868          {
00869 // start slave bug prevention code, remove if possible            
00870             if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave!
00871             {   
00872                SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4
00873             }
00874             if(tSM)
00875             {   
00876                tSM += SMt_bug_add; // only add if SMt > 0
00877             }
00878             if((iSM == 2) && (tSM == 0)) // SM2 has type 0, this is a bug in the slave!
00879             {   
00880                tSM = 3;
00881             }
00882             if((iSM == 3) && (tSM == 0)) // SM3 has type 0, this is a bug in the slave!
00883             {   
00884                tSM = 4;
00885             }
00886 // end slave bug prevention code            
00887 
00888             context->slavelist[Slave].SMtype[iSM] = tSM;
00889             /* check if SM is unused -> clear enable flag */
00890             if (tSM == 0)
00891             {
00892                context->slavelist[Slave].SM[iSM].SMflags = 
00893                   htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK);
00894             }
00895             if ((tSM == 3) || (tSM == 4))
00896             {
00897                /* read the assign PDO */
00898                Tsize = ecx_readPDOassign(context, Slave, ECT_SDO_PDOASSIGN + iSM );
00899                /* if a mapping is found */
00900                if (Tsize)
00901                {
00902                   context->slavelist[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8);
00903                   if (tSM == 3)
00904                   {  
00905                      /* we are doing outputs */
00906                      *Osize += Tsize;
00907                   }
00908                   else
00909                   {
00910                      /* we are doing inputs */
00911                      *Isize += Tsize;
00912                   }   
00913                }   
00914             }   
00915          }   
00916       }
00917    }
00918 
00919    /* found some I/O bits ? */
00920    if ((*Isize > 0) || (*Osize > 0))
00921    {
00922       retVal = 1;
00923    }
00924       
00925    return retVal;
00926 }
00927 
00940 int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize)
00941 {
00942    int wkc, rdl;
00943    int retVal = 0;
00944    uint8 nSM, iSM, tSM;
00945    int Tsize;
00946    uint8 SMt_bug_add;
00947    
00948    *Isize = 0;
00949    *Osize = 0;
00950    SMt_bug_add = 0;
00951    rdl = sizeof(ec_SMcommtypet); 
00952    context->SMcommtype->n = 0;
00953    /* read SyncManager Communication Type object count Complete Access*/
00954    wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, TRUE, &rdl, context->SMcommtype, EC_TIMEOUTRXM);
00955    /* positive result from slave ? */
00956    if ((wkc > 0) && (context->SMcommtype->n > 2))
00957    {
00958       /* make nSM equal to number of defined SM */
00959       nSM = context->SMcommtype->n - 1;
00960       /* limit to maximum number of SM defined, if true the slave can't be configured */
00961       if (nSM > EC_MAXSM)
00962       {
00963          nSM = EC_MAXSM;
00964          ecx_packeterror(context, Slave, 0, 0, 10); /* #SM larger than EC_MAXSM */         
00965       }
00966       /* iterate for every SM type defined */
00967       for (iSM = 2 ; iSM <= nSM ; iSM++)
00968       {
00969           tSM = context->SMcommtype->SMtype[iSM];
00970 
00971 // start slave bug prevention code, remove if possible            
00972          if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave!
00973          {
00974             SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4
00975          }
00976          if(tSM)
00977          {
00978             tSM += SMt_bug_add; // only add if SMt > 0
00979          }
00980 // end slave bug prevention code
00981          
00982          context->slavelist[Slave].SMtype[iSM] = tSM;
00983          /* check if SM is unused -> clear enable flag */
00984          if (tSM == 0)
00985          {
00986             context->slavelist[Slave].SM[iSM].SMflags =
00987                htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK);
00988          }
00989          if ((tSM == 3) || (tSM == 4))
00990          {
00991             /* read the assign PDO */
00992             Tsize = ecx_readPDOassignCA(context, Slave, ECT_SDO_PDOASSIGN + iSM );
00993             /* if a mapping is found */
00994             if (Tsize)
00995             {
00996                context->slavelist[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8);
00997                if (tSM == 3)
00998                {
00999                   /* we are doing outputs */
01000                   *Osize += Tsize;
01001                }
01002                else
01003                {
01004                   /* we are doing inputs */
01005                   *Isize += Tsize;
01006                }
01007             }   
01008          }   
01009       }
01010    }
01011 
01012    /* found some I/O bits ? */
01013    if ((*Isize > 0) || (*Osize > 0))
01014    {
01015       retVal = 1;
01016    }
01017    return retVal;
01018 }
01019 
01027 int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist)
01028 {
01029    ec_SDOservicet *SDOp, *aSDOp;
01030    ec_mbxbuft MbxIn, MbxOut;
01031    int wkc;
01032    uint16 x, n, i, sp, offset;
01033    boolean stop;
01034    uint8 cnt;
01035    boolean First;
01036 
01037    pODlist->Slave = Slave;
01038    pODlist->Entries = 0;
01039    ec_clearmbx(&MbxIn);
01040    /* clear pending out mailbox in slave if available. Timeout is set to 0 */
01041    wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
01042    ec_clearmbx(&MbxOut);
01043    aSDOp = (ec_SDOservicet*)&MbxIn;
01044    SDOp = (ec_SDOservicet*)&MbxOut;
01045    SDOp->MbxHeader.length = htoes(0x0008);
01046    SDOp->MbxHeader.address = htoes(0x0000);
01047    SDOp->MbxHeader.priority = 0x00;
01048    /* Get new mailbox counter value */
01049    cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
01050    context->slavelist[Slave].mbx_cnt = cnt;
01051    SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
01052    SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
01053    SDOp->Opcode = ECT_GET_ODLIST_REQ; /* get object description list request */
01054    SDOp->Reserved = 0;
01055    SDOp->Fragments = 0; /* fragments left */
01056    SDOp->wdata[0] = htoes(0x01); /* all objects */
01057    /* send get object description list request to slave */
01058    wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
01059    /* mailbox placed in slave ? */
01060    if (wkc > 0)
01061    {
01062       x = 0; 
01063       sp = 0;
01064       First = TRUE;
01065       offset = 1; /* offset to skip info header in first frame, otherwise set to 0 */
01066       do
01067       {
01068          stop = TRUE; /* assume this is last iteration */
01069          ec_clearmbx(&MbxIn);
01070          /* read slave response */
01071          wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
01072          /* got response ? */
01073          if (wkc > 0)
01074          {
01075             /* response should be CoE and "get object description list response" */
01076             if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
01077                 ((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES))
01078             {
01079                if (First)
01080                {   
01081                   /* extract number of indexes from mailbox data size */
01082                   n = (etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2;
01083                }
01084                else
01085                {
01086                   /* extract number of indexes from mailbox data size */
01087                   n = (etohs(aSDOp->MbxHeader.length) - 6) / 2;
01088                }
01089                /* check if indexes fit in buffer structure */
01090                if ((sp + n) > EC_MAXODLIST)
01091                {
01092                   n = EC_MAXODLIST + 1 - sp;
01093                   ecx_SDOinfoerror(context, Slave, 0, 0, 0xf000000); /* Too many entries for master buffer */
01094                   stop = TRUE;
01095                }
01096                /* trim to maximum number of ODlist entries defined */
01097                if ((pODlist->Entries + n) > EC_MAXODLIST)
01098                {
01099                   n = EC_MAXODLIST - pODlist->Entries;
01100                }
01101                pODlist->Entries += n;
01102                /* extract indexes one by one */
01103                for (i = 0; i < n; i++)
01104                {
01105                   pODlist->Index[sp + i] = etohs(aSDOp->wdata[i + offset]);
01106                }
01107                sp += n;
01108                /* check if more fragments will follow */
01109                if (aSDOp->Fragments > 0)
01110                {   
01111                   stop = FALSE;
01112                }
01113                First = FALSE; 
01114                offset = 0;
01115             }
01116             /* got unexpected response from slave */
01117             else
01118             {
01119                if ((aSDOp->Opcode &  0x7f) == ECT_SDOINFO_ERROR) /* SDO info error received */
01120                {
01121                   ecx_SDOinfoerror(context, Slave, 0, 0, etohl(aSDOp->ldata[0]));
01122                   stop = TRUE;
01123                }
01124                else
01125                {
01126                   ecx_packeterror(context, Slave, 0, 0, 1); /* Unexpected frame returned */
01127                }
01128                wkc = 0; 
01129                x += 20;
01130             }
01131          }
01132          x++;
01133       }
01134       while ((x <= 128) && !stop);
01135    }
01136    return wkc;
01137 }
01138 
01146 int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist)
01147 {
01148    ec_SDOservicet *SDOp, *aSDOp;
01149    int wkc;
01150    uint16  n, Slave;
01151    ec_mbxbuft MbxIn, MbxOut;
01152    uint8 cnt;
01153 
01154    Slave = pODlist->Slave;
01155    pODlist->DataType[Item] = 0;
01156    pODlist->ObjectCode[Item] = 0;
01157    pODlist->MaxSub[Item] = 0;
01158    pODlist->Name[Item][0] = 0;
01159    ec_clearmbx(&MbxIn);
01160    /* clear pending out mailbox in slave if available. Timeout is set to 0 */
01161    wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
01162    ec_clearmbx(&MbxOut);
01163    aSDOp = (ec_SDOservicet*)&MbxIn;
01164    SDOp = (ec_SDOservicet*)&MbxOut;
01165    SDOp->MbxHeader.length = htoes(0x0008);
01166    SDOp->MbxHeader.address = htoes(0x0000);
01167    SDOp->MbxHeader.priority = 0x00;
01168    /* Get new mailbox counter value */
01169    cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
01170    context->slavelist[Slave].mbx_cnt = cnt;
01171    SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
01172    SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
01173    SDOp->Opcode = ECT_GET_OD_REQ; /* get object description request */
01174    SDOp->Reserved = 0;
01175    SDOp->Fragments = 0; /* fragments left */
01176    SDOp->wdata[0] = htoes(pODlist->Index[Item]); /* Data of Index */
01177    /* send get object description request to slave */
01178    wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
01179    /* mailbox placed in slave ? */
01180    if (wkc > 0)
01181    {
01182       ec_clearmbx(&MbxIn);
01183       /* read slave response */
01184       wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
01185       /* got response ? */
01186       if (wkc > 0)
01187       {
01188          if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
01189              ((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES))
01190          {
01191             n = (etohs(aSDOp->MbxHeader.length) - 12); /* length of string(name of object) */
01192             if (n > EC_MAXNAME)
01193             {
01194                n = EC_MAXNAME; /* max chars */
01195             }
01196             pODlist->DataType[Item] = etohs(aSDOp->wdata[1]);
01197             pODlist->ObjectCode[Item] = aSDOp->bdata[5];
01198             pODlist->MaxSub[Item] = aSDOp->bdata[4];
01199 
01200             strncpy(pODlist->Name[Item] , (char *)&aSDOp->bdata[6], n);
01201             pODlist->Name[Item][n] = 0x00; /* String terminator */
01202          }
01203          /* got unexpected response from slave */
01204          else
01205          {
01206             if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */
01207             {
01208                ecx_SDOinfoerror(context, Slave,pODlist->Index[Item], 0, etohl(aSDOp->ldata[0]));
01209             }
01210             else
01211             {
01212                ecx_packeterror(context, Slave,pODlist->Index[Item], 0, 1); /* Unexpected frame returned */
01213             }
01214             wkc = 0;
01215          }
01216       }
01217    }
01218   
01219    return wkc;
01220 }
01221 
01232 int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist)
01233 {
01234    ec_SDOservicet *SDOp, *aSDOp;
01235    uint16 wkc, Index, Slave;
01236    int16 n;
01237    ec_mbxbuft MbxIn, MbxOut;
01238    uint8 cnt;
01239 
01240    wkc = 0;
01241    Slave = pODlist->Slave;
01242    Index = pODlist->Index[Item];
01243    ec_clearmbx(&MbxIn);
01244    /* clear pending out mailbox in slave if available. Timeout is set to 0 */
01245    wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
01246    ec_clearmbx(&MbxOut);
01247    aSDOp = (ec_SDOservicet*)&MbxIn;
01248    SDOp = (ec_SDOservicet*)&MbxOut;
01249    SDOp->MbxHeader.length = htoes(0x000a);
01250    SDOp->MbxHeader.address = htoes(0x0000);
01251    SDOp->MbxHeader.priority = 0x00;
01252    /* Get new mailbox counter value */
01253    cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
01254    context->slavelist[Slave].mbx_cnt = cnt;
01255    SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
01256    SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
01257    SDOp->Opcode = ECT_GET_OE_REQ; /* get object entry description request */
01258    SDOp->Reserved = 0;
01259    SDOp->Fragments = 0;      /* fragments left */
01260    SDOp->wdata[0] = htoes(Index);      /* Index */
01261    SDOp->bdata[2] = SubI;       /* SubIndex */
01262    SDOp->bdata[3] = 1 + 2 + 4; /* get access rights, object category, PDO */
01263    /* send get object entry description request to slave */
01264    wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
01265    /* mailbox placed in slave ? */
01266    if (wkc > 0)
01267    {
01268       ec_clearmbx(&MbxIn);
01269       /* read slave response */
01270       wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
01271       /* got response ? */
01272       if (wkc > 0)
01273       {
01274          if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
01275              ((aSDOp->Opcode &  0x7f) == ECT_GET_OE_RES))
01276          {
01277             pOElist->Entries++;
01278             n = (etohs(aSDOp->MbxHeader.length) - 16); /* length of string(name of object) */
01279             if (n > EC_MAXNAME)
01280             {
01281                n = EC_MAXNAME; /* max string length */
01282             }
01283             if (n < 0 )
01284             {
01285                n = 0;
01286             }
01287             pOElist->ValueInfo[SubI] = aSDOp->bdata[3];
01288             pOElist->DataType[SubI] = etohs(aSDOp->wdata[2]);
01289             pOElist->BitLength[SubI] = etohs(aSDOp->wdata[3]);
01290             pOElist->ObjAccess[SubI] = etohs(aSDOp->wdata[4]);
01291 
01292             strncpy(pOElist->Name[SubI] , (char *)&aSDOp->wdata[5], n);
01293             pOElist->Name[SubI][n] = 0x00; /* string terminator */
01294          }
01295          /* got unexpected response from slave */
01296          else
01297          {
01298             if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */
01299             {
01300                ecx_SDOinfoerror(context, Slave, Index, SubI, etohl(aSDOp->ldata[0]));
01301             }
01302             else
01303             {
01304                ecx_packeterror(context, Slave, Index, SubI, 1); /* Unexpected frame returned */
01305             }
01306             wkc = 0;
01307          }
01308       }
01309    }
01310    
01311    return wkc;
01312 }
01313 
01322 int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist)
01323 {
01324    uint16 SubCount;
01325    int wkc;
01326    uint8 SubI;
01327 
01328    wkc = 0;
01329    pOElist->Entries = 0;
01330    SubI = pODlist->MaxSub[Item];
01331    /* for each entry found in ODlist */
01332    for (SubCount = 0; SubCount <= SubI; SubCount++)
01333    {
01334       /* read subindex of entry */
01335       wkc = ecx_readOEsingle(context, Item, (uint8)SubCount, pODlist, pOElist);
01336    }
01337    
01338    return wkc;
01339 }
01340 
01341 #ifdef EC_VER1
01342 void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
01343 {
01344    ecx_SDOerror(&ecx_context, Slave, Index, SubIdx, AbortCode);
01345 }
01346 
01347 int ec_SDOread(uint16 slave, uint16 index, uint8 subindex,
01348                boolean CA, int *psize, void *p, int timeout)
01349 {
01350    return ecx_SDOread(&ecx_context, slave, index, subindex, CA, psize, p, timeout);
01351 }
01352 
01353 int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex,
01354                 boolean CA, int psize, void *p, int Timeout)
01355 {
01356    return ecx_SDOwrite(&ecx_context, Slave, Index, SubIndex, CA, psize, p, Timeout);
01357 }
01358 
01359 int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber, int psize, void *p)
01360 {
01361    return ecx_RxPDO(&ecx_context, Slave, RxPDOnumber, psize, p);
01362 }
01363 
01364 int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout)
01365 {
01366    return ecx_TxPDO(&ecx_context, slave, TxPDOnumber, psize, p, timeout);
01367 }
01368 
01370 int ec_readPDOassign(uint16 Slave, uint16 PDOassign)
01371 {
01372    return ecx_readPDOassign(&ecx_context, Slave, PDOassign);
01373 }
01374 
01376 int ec_readPDOassignCA(uint16 Slave, uint16 PDOassign)
01377 {
01378    return ecx_readPDOassignCA(&ecx_context, Slave, PDOassign);
01379 }
01380 
01381 int ec_readPDOmap(uint16 Slave, int *Osize, int *Isize)
01382 {
01383    return ecx_readPDOmap(&ecx_context, Slave, Osize, Isize);
01384 }
01385 
01386 int ec_readPDOmapCA(uint16 Slave, int *Osize, int *Isize)
01387 {
01388    return ecx_readPDOmapCA(&ecx_context, Slave, Osize, Isize);
01389 }
01390 
01391 int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist)
01392 {
01393    return ecx_readODlist(&ecx_context, Slave, pODlist);
01394 }
01395 
01396 int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist)
01397 {
01398    return ecx_readODdescription(&ecx_context, Item, pODlist);
01399 }
01400 
01401 int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist)
01402 {
01403    return ecx_readOEsingle(&ecx_context, Item, SubI, pODlist, pOElist);
01404 }
01405 
01406 int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist)
01407 {
01408    return ecx_readOE(&ecx_context, Item, pODlist, pOElist);
01409 }
01410 #endif


ethercat_soem
Author(s): Arthur Ketels, M.J.G. van de Molengraft
autogenerated on Wed Aug 26 2015 11:32:40