1

/*

     2

  This file is part of CanFestival, a library implementing CanOpen

     3

  Stack.

     4

     5

  Copyright (C): Edouard TISSERANT and Francis DUPIN

     6

     7

  See COPYING file for copyrights details.

     8

     9

  This library is free software; you can redistribute it and/or

    10

  modify it under the terms of the GNU Lesser General Public

    11

  License as published by the Free Software Foundation; either

    12

  version 2.1 of the License, or (at your option) any later version.

    13

    14

  This library is distributed in the hope that it will be useful,

    15

  but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    17

  Lesser General Public License for more details.

    18

    19

  You should have received a copy of the GNU Lesser General Public

    20

  License along with this library; if not, write to the Free Software

    21

  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307

    22

  USA

    23

*/

    24

    25

    26

/**

    27

** @file   dcf.c

    28

** @author Edouard TISSERANT and Francis DUPIN

    29

** @date   Mon Jun  4 17:06:12 2007

    30

**

    31

** @brief EXEMPLE OF SOMMARY

    32

**

    33

**

    34

*/

    35

    36

    37

#include "data.h"

    38

#include "sysdep.h"

    39

    40

extern UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,

    41

                               UNS8 subIndex, UNS8 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize);

    42

    43

    44

static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId);

    45

    46

/* Seek to next NodeID's DCF */

    47

#define SEEK_NEXT_DCF() \

    48

   	nodeId=(nodeId+1) % d->dcf_odentry->bSubCount; \

    49

   	if(nodeId==0) nodeId=1; \

    50

   	d->dcf_cursor = NULL;

    51

    52

/**

    53

**

    54

**

    55

** @param d

    56

** @param nodeId

    57

*/

    58

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    59

{

    60

  UNS32 abortCode = 0;

    61

    62

  if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)

    63

    {

    64

      MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);

    65

      MSG_WAR(0x2A02, "server node : ", nodeId);

    66

    }

    67

    68

  closeSDOtransfer(d, nodeId, SDO_CLIENT);

    69

  /* Timedout ? */

    70

  if(abortCode == SDOABT_TIMED_OUT){

    71

    /* Node may not be ready, try another one */

    72

    /* Warning, this might leed to endless attempts */

    73

    /* if node does never answer */

    74

	SEEK_NEXT_DCF()

    75

  }

    76

  send_consise_dcf_loop(d,nodeId);

    77

}

    78

    79

    80

/**

    81

**

    82

**

    83

** @param d

    84

** @param nodeId

    85

**

    86

** @return

    87

*/

    88

UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId)

    89

{

    90

  UNS32 szData;

    91

  /* Fetch DCF OD entry, if not already done */

    92

  if(!d->dcf_odentry)

    93

  {

    94

    UNS32 errorCode;

    95

    ODCallback_t *Callback;

    96

    d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);

    97

    /* If DCF entry do not exist... Nothing to do.*/

    98

    if (errorCode != OD_SUCCESSFUL) goto DCF_finish;

    99

  }

   100

   101

  szData = d->dcf_odentry->pSubindex[nodeId].size;

   102

   103

  /* if the entry for the nodeId is not empty. */

   104

  if(szData!=0){

   105

  	/* if the entry for the nodeId is already been processing, quit.*/

   106

  	if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) return 1;

   107

   108

  	d->dcf_odentry->pSubindex[nodeId].bAccessType|=DCF_TO_SEND;

   109

  	d->dcf_request++;

   110

  	if(d->dcf_request==1)

   111

  		send_consise_dcf_loop(d,nodeId);

   112

  	return 1;

   113

  }

   114

   115

  DCF_finish:

   116

  return 0;

   117

}

   118

   119

static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)

   120

{

   121

  if(nodeId > d->dcf_odentry->bSubCount) return;

   122

/* Loop on all DCF subindexes, corresponding to node ID until there is no request*/

   123

  //while (nodeId < d->dcf_odentry->bSubCount){

   124

  while (d->dcf_request>0){

   125

  	if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND){   	 

   126

        UNS8* dcfend;

   127

  		UNS32 nb_entries;

   128

  		UNS32 szData = d->dcf_odentry->pSubindex[nodeId].size;

   129

   130

   		{

   131

	   		UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject);

   132

   			dcfend = dcf + szData;

   133

	   		if (!d->dcf_cursor){

   134

    	  		d->dcf_cursor = (UNS8*)dcf + 4;

   135

       			d->dcf_entries_count = 0;

   136

   			}

   137

   			nb_entries = UNS32_LE(*((UNS32*)dcf));

   138

   		}

   139

   140

    	/* condition on consise DCF string for NodeID, if big enough */

   141

    	if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){

   142

   143

        	UNS16 target_Index;

   144

        	UNS8 target_Subindex;

   145

        	UNS32 target_Size;

   146

   147

			/* DCF data may not be 32/16b aligned, 

   148

			 * we cannot directly dereference d->dcf_cursor 

   149

			 * as UNS16 or UNS32 

   150

			 * Do it byte per byte taking care on endianess*/

   151

#ifdef CANOPEN_BIG_ENDIAN

   152

        	target_Index = *(d->dcf_cursor++) << 8 | 

   153

        	               *(d->dcf_cursor++);

   154

#else

   155

        	memcpy(&target_Index, d->dcf_cursor,2);

   156

        	d->dcf_cursor+=2;

   157

#endif

   158

   159

        	target_Subindex = *(d->dcf_cursor++);

   160

   161

#ifdef CANOPEN_BIG_ENDIAN

   162

        	target_Size = *(d->dcf_cursor++) << 24 | 

   163

        	              *(d->dcf_cursor++) << 16 | 

   164

        	              *(d->dcf_cursor++) << 8 | 

   165

        	              *(d->dcf_cursor++);

   166

#else

   167

        	memcpy(&target_Size, d->dcf_cursor,4);

   168

        	d->dcf_cursor+=4;

   169

#endif

   170

   171

    	    _writeNetworkDict(d, /* CO_Data* d*/

   172

                                nodeId, /* UNS8 nodeId*/

   173

                                target_Index, /* UNS16 index*/

   174

                                target_Subindex, /* UNS8 subindex*/

   175

                                (UNS8)target_Size, /* UNS8 count*/

   176

                                0, /* UNS8 dataType*/

   177

                                d->dcf_cursor,/* void *data*/

   178

                                CheckSDOAndContinue,/* SDOCallback_t

   179

                                                      Callback*/

   180

                                0); /* no endianize*/

   181

        	/* Push d->dcf_cursor to the end of data*/

   182

   183

        	d->dcf_cursor += target_Size;

   184

        	d->dcf_entries_count++;

   185

   186

        	/* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/

   187

        	return;

   188

      	}

   189

      	else

   190

      	{

   191

      		/* We have finished with the dcf entry. Change the flag, decrement the request

   192

      		 *  and execute the bootup callback. */

   193

      		d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND;

   194

      		d->dcf_request--;

   195

      		(*d->post_SlaveBootup)(d, nodeId);

   196

      	}

   197

 	}

   198

   199

	SEEK_NEXT_DCF()

   200

  }

   201

   202

}