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

  UNS8 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

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

   122

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

   123

  while (d->dcf_request>0){

   124

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

   125

        UNS8* dcfend;

   126

  		UNS32 nb_entries;

   127

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

   128

   129

   		{

   130

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

   131

   			dcfend = dcf + szData;

   132

	   		if (!d->dcf_cursor){

   133

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

   134

       			d->dcf_entries_count = 0;

   135

   			}

   136

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

   137

   		}

   138

   139

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

   140

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

   141

   142

        	UNS16 target_Index;

   143

        	UNS8 target_Subindex;

   144

        	UNS32 target_Size;

   145

   146

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

   147

			 * we cannot directly dereference d->dcf_cursor 

   148

			 * as UNS16 or UNS32 

   149

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

   150

#ifdef CANOPEN_BIG_ENDIAN

   151

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

   152

        	               *(d->dcf_cursor++);

   153

#else

   154

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

   155

        	d->dcf_cursor+=2;

   156

#endif

   157

   158

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

   159

   160

#ifdef CANOPEN_BIG_ENDIAN

   161

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

   162

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

   163

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

   164

        	              *(d->dcf_cursor++);

   165

#else

   166

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

   167

        	d->dcf_cursor+=4;

   168

#endif

   169

   170

    	    _writeNetworkDict(d, /* CO_Data* d*/

   171

                                nodeId, /* UNS8 nodeId*/

   172

                                target_Index, /* UNS16 index*/

   173

                                target_Subindex, /* UNS8 subindex*/

   174

                                target_Size, /* UNS8 count*/

   175

                                0, /* UNS8 dataType*/

   176

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

   177

                                CheckSDOAndContinue,/* SDOCallback_t

   178

                                                      Callback*/

   179

                                0); /* no endianize*/

   180

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

   181

   182

        	d->dcf_cursor += target_Size;

   183

        	d->dcf_entries_count++;

   184

   185

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

   186

        	return;

   187

      	}

   188

      	else

   189

      	{

   190

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

   191

      		 *  and execute the bootup callback. */

   192

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

   193

      		d->dcf_request--;

   194

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

   195

      	}

   196

 	}

   197

   198

	SEEK_NEXT_DCF()

   199

  }

   200

   201

}