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 "objacces.h"

    38

#include "sdo.h"

    39

#include "dcf.h"

    40

#include "sysdep.h"

    41

    42

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

    43

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

    44

    45

    46

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

    47

    48

/**

    49

**

    50

**

    51

** @param d

    52

** @param nodeId

    53

*/

    54

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    55

{

    56

  UNS32 abortCode;

    57

    58

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

    59

    {

    60

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

    61

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

    62

    }

    63

    64

  closeSDOtransfer(d, nodeId, SDO_CLIENT);

    65

  send_consise_dcf_loop(d,nodeId);

    66

}

    67

    68

    69

/**

    70

**

    71

**

    72

** @param d

    73

** @param nodeId

    74

**

    75

** @return

    76

*/

    77

UNS8 send_consise_dcf(CO_Data* d,UNS8 nodeId)

    78

{

    79

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

    80

  if(!d->dcf_odentry)

    81

  {

    82

    UNS32 errorCode;

    83

    ODCallback_t *Callback;

    84

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

    85

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

    86

    if (errorCode != OD_SUCCESSFUL) goto DCF_finish;

    87

  }

    88

    89

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

    90

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

    91

  if(szData!=0){

    92

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

    93

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

    94

    95

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

    96

  	d->dcf_request++;

    97

  	if(d->dcf_request==1)

    98

  		send_consise_dcf_loop(d,nodeId);

    99

  	return 1;

   100

  }

   101

   102

  DCF_finish:

   103

  return 0;

   104

}

   105

   106

static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)

   107

{

   108

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

   109

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

   110

  while (d->dcf_request>0){

   111

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

   112

  		UNS32 nb_entries;

   113

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

   114

   		UNS8* dcfend;

   115

   116

   		{

   117

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

   118

   			dcfend = dcf + szData;

   119

	   		if (!d->dcf_cursor){

   120

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

   121

       			d->dcf_entries_count = 0;

   122

   			}

   123

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

   124

   		}

   125

   126

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

   127

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

   128

   129

        	UNS16 target_Index;

   130

        	UNS8 target_Subindex;

   131

        	UNS32 target_Size;

   132

   133

        	/* pointer to the DCF string for NodeID */

   134

        	target_Index = UNS16_LE(*((UNS16*)(d->dcf_cursor))); d->dcf_cursor += 2;

   135

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

   136

        	target_Size = UNS32_LE(*((UNS32*)(d->dcf_cursor))); d->dcf_cursor += 4;

   137

   138

    	    _writeNetworkDict(d, /* CO_Data* d*/

   139

                                nodeId, /* UNS8 nodeId*/

   140

                                target_Index, /* UNS16 index*/

   141

                                target_Subindex, /* UNS8 subindex*/

   142

                                target_Size, /* UNS8 count*/

   143

                                0, /* UNS8 dataType*/

   144

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

   145

                                CheckSDOAndContinue,/* SDOCallback_t

   146

                                                      Callback*/

   147

                                0); /* no endianize*/

   148

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

   149

   150

        	d->dcf_cursor += target_Size;

   151

        	d->dcf_entries_count++;

   152

   153

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

   154

        	return;

   155

      	}

   156

      	else

   157

      	{

   158

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

   159

      		 *  and execute the bootup callback. */

   160

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

   161

      		d->dcf_request--;

   162

      		(*d->post_SlaveBootup)(nodeId);

   163

      	}

   164

 	}

   165

   166

   	/* Check the next element*/

   167

   	//nodeId++;

   168

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

   169

   	if(nodeId==d->dcf_odentry->bSubCount)nodeId=1;

   170

   	d->dcf_cursor = NULL;

   171

   172

  }

   173

   174

  //DCF_finish:

   175

  /*  Switch Master to preOperational state */

   176

   177

  //(*d->preOperational)();

   178

}