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

  UNS8 szData;

    80

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

    81

  if(!d->dcf_odentry)

    82

  {

    83

    UNS32 errorCode;

    84

    ODCallback_t *Callback;

    85

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

    86

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

    87

    if (errorCode != OD_SUCCESSFUL) goto DCF_finish;

    88

  }

    89

    90

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

    91

    92

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

    93

  if(szData!=0){

    94

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

    95

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

    96

    97

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

    98

  	d->dcf_request++;

    99

  	if(d->dcf_request==1)

   100

  		send_consise_dcf_loop(d,nodeId);

   101

  	return 1;

   102

  }

   103

   104

  DCF_finish:

   105

  return 0;

   106

}

   107

   108

static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)

   109

{

   110

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

   111

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

   112

  while (d->dcf_request>0){

   113

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

   114

        UNS8* dcfend;

   115

  		UNS32 nb_entries;

   116

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

   117

   118

   		{

   119

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

   120

   			dcfend = dcf + szData;

   121

	   		if (!d->dcf_cursor){

   122

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

   123

       			d->dcf_entries_count = 0;

   124

   			}

   125

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

   126

   		}

   127

   128

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

   129

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

   130

   131

        	UNS16 target_Index;

   132

        	UNS8 target_Subindex;

   133

        	UNS32 target_Size;

   134

   135

        	/* pointer to the DCF string for NodeID */

   136

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

   137

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

   138

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

   139

   140

    	    _writeNetworkDict(d, /* CO_Data* d*/

   141

                                nodeId, /* UNS8 nodeId*/

   142

                                target_Index, /* UNS16 index*/

   143

                                target_Subindex, /* UNS8 subindex*/

   144

                                target_Size, /* UNS8 count*/

   145

                                0, /* UNS8 dataType*/

   146

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

   147

                                CheckSDOAndContinue,/* SDOCallback_t

   148

                                                      Callback*/

   149

                                0); /* no endianize*/

   150

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

   151

   152

        	d->dcf_cursor += target_Size;

   153

        	d->dcf_entries_count++;

   154

   155

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

   156

        	return;

   157

      	}

   158

      	else

   159

      	{

   160

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

   161

      		 *  and execute the bootup callback. */

   162

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

   163

      		d->dcf_request--;

   164

      		(*d->post_SlaveBootup)(nodeId);

   165

      	}

   166

 	}

   167

   168

   	/* Check the next element*/

   169

   	//nodeId++;

   170

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

   171

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

   172

   	d->dcf_cursor = NULL;

   173

   174

  }

   175

   176

  //DCF_finish:

   177

  /*  Switch Master to preOperational state */

   178

   179

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

   180

}