39

#include "dcf.h"

    40

    41

typedef struct {

    42

    UNS16 Index;

    43

    UNS8 Subindex;

    44

    UNS32 Size;

    45

    UNS8 *Data;

    46

} dcf_entry_t;

    47

    48

void SaveNode(CO_Data* d, UNS8 nodeId);

    49

static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);

    50

static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId);

    51

UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId);

    52

    53

/**

    54

** @brief Function to be called from post_SlaveBootup

    59

UNS8 check_and_start_node(CO_Data* d, UNS8 nodeId)

    60

{   

    61

    if(d->dcf_status != DCF_STATUS_INIT)

    62

        return 0;

    63

    if((init_consise_dcf(d, nodeId) == 0) || (read_consise_dcf_next_entry(d, nodeId) == 0)){

    64

	    /* Ask slave node to go in operational mode */

    65

        masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

    66

        d->NMTable[nodeId] = Operational;

    67

        return 1;

    68

    }

    69

    d->dcf_status = DCF_STATUS_READ_CHECK;

    70

    return 2;

    71

}

    78

*/

    79

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    80

{

    81

    UNS32 abortCode = 0;

    82

    UNS8 buf[4], match = 0, node;

    83

    UNS32 size=4;

    84

    if(d->dcf_status == DCF_STATUS_READ_CHECK){

    85

        // printf("DCF_STATUS_READ_CHECK \n");

    86

        if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED)

    87

            goto dcferror;

    88

        /* Check if data received match the DCF */

    89

        if(size == d->dcf_size){

    90

            match = 1;

    91

            while(--size)

    92

                if(buf[size] != d->dcf_data[size])

    93

                    match = 0;

    94

        }

    95

        if(match) {

    96

            if(read_consise_dcf_next_entry(d, nodeId) == 0){

    97

                masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

    98

                d->NMTable[nodeId] = Operational;

    99

                d->dcf_status = DCF_STATUS_INIT;

   100

                /* Look for other nodes waiting to be started */

   101

                for(node = 0 ; node<NMT_MAX_NODE_ID ; node++){

   102

                    if(d->NMTable[node] != Initialisation)

   103

                        continue;

   104

                    if(check_and_start_node(d, node) == 2)

   105

                        break;

   106

                }

   107

            }

   108

        }

   109

        else { /* Data received does not match : start rewriting all */

   110

            if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0))

   111

                goto dcferror;                

   112

            d->dcf_status = DCF_STATUS_WRITE;

   113

        }

   114

    }

   115

    else if(d->dcf_status == DCF_STATUS_WRITE){

   116

        // printf("DCF_STATUS_WRITE \n");

   117

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

   118

            goto dcferror;

   119

        if(write_consise_dcf_next_entry(d, nodeId) == 0){

   120

            SaveNode(d, nodeId);

   121

            d->dcf_status = DCF_STATUS_SAVED;

   122

        }

   123

    }

   124

    else if(d->dcf_status == DCF_STATUS_SAVED){

   125

        // printf("DCF_STATUS_SAVED \n");

   126

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

   127

            goto dcferror;

   128

        masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);

   129

        d->dcf_status = DCF_STATUS_INIT;

   130

        d->NMTable[nodeId] = Unknown_state;

   131

    }

   132

    return;

   133

dcferror:

   134

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

   135

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

   136

    d->NMTable[nodeId] = Unknown_state;

   137

}

   138

   139

/**

   140

**

   141

**

   142

** @param d

   143

** @param nodeId

   147

UNS8 init_consise_dcf(CO_Data* d,UNS8 nodeId)

   148

{

   149

    /* Fetch DCF OD entry */

   155

    /* Fix DCF table overflow */

   156

    if(nodeId > d->dcf_odentry->bSubCount) goto DCF_finish;

   157

    /* If DCF empty... Nothing to do */

   158

    if(! d->dcf_odentry->pSubindex[nodeId].size) goto DCF_finish;

   159

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

   160

    // printf("%.2x %.2x %.2x %.2x\n",dcf[0],dcf[1],dcf[2],dcf[3]);

   161

    d->dcf_cursor = dcf + 4;

   162

    d->dcf_entries_count = 0;

   163

    d->dcf_status = DCF_STATUS_INIT;

   164

    return 1;

   165

    DCF_finish:

   166

    return 0;

   167

}

   168

   169

UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId)

   170

{

   171

  UNS8* dcfend;

   172

  UNS32 nb_entries;

   173

  UNS32 szData;

   174

  if(!d->dcf_odentry)

   175

     return 0;

   176

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

   177

     return 0;

   178

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

   179

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

   180

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

   181

  dcfend = dcf + szData;

   182

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

   183

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

   184

    * we cannot directly dereference d->dcf_cursor 

   185

    * as UNS16 or UNS32 

   186

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

   187

#ifdef CANOPEN_BIG_ENDIAN

   188

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

   189

     	               *(d->dcf_cursor++);

   190

#else

   191

    memcpy(&dcf_entry->Index, d->dcf_cursor,2);

   192

       	d->dcf_cursor+=2;

   193

#endif

   194

    dcf_entry->Subindex = *(d->dcf_cursor++);

   195

#ifdef CANOPEN_BIG_ENDIAN

   196

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

   197

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

   198

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

   199

        	          *(d->dcf_cursor++);

   200

#else

   201

    memcpy(&dcf_entry->Size, d->dcf_cursor,4);

   202

    d->dcf_cursor+=4;

   203

#endif

   204

    d->dcf_data = dcf_entry->Data = d->dcf_cursor;

   205

    d->dcf_size = dcf_entry->Size;

   206

    d->dcf_cursor += dcf_entry->Size;

   207

    d->dcf_entries_count++;

   208

    return 1;

   213

static UNS8 write_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)

   214

{

   215

    UNS8 Ret;

   216

    dcf_entry_t dcf_entry;

   217

    if(!get_next_DCF_data(d, &dcf_entry, nodeId))

   218

        return 0;

   219

    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/

   220

                    nodeId, /* UNS8 nodeId*/

   221

                    dcf_entry.Index, /* UNS16 index*/

   222

                    dcf_entry.Subindex, /* UNS8 subindex*/

   223

                    (UNS8)dcf_entry.Size, /* UNS8 count*/

   224

                    0, /* UNS8 dataType*/

   225

                    dcf_entry.Data,/* void *data*/

   226

                    CheckSDOAndContinue,/* Callback*/

   227

                    0,   /* no endianize*/

   228

                    0); /* no block mode */

   229

    if(Ret)

   230

        MSG_ERR(0x1A02,"Erreur writeNetworkDictCallBackAI",Ret);

   231

    return 1;

   232

}

   233

   234

static UNS8 read_consise_dcf_next_entry(CO_Data* d, UNS8 nodeId)

   235

{

   236

    UNS8 Ret;

   237

    dcf_entry_t dcf_entry;

   238

    if(!get_next_DCF_data(d, &dcf_entry, nodeId))

   239

        return 0;

   240

    Ret = readNetworkDictCallbackAI(d, /* CO_Data* d*/

   241

                   nodeId, /* UNS8 nodeId*/

   242

                   dcf_entry.Index, /* UNS16 index*/

   243

                   dcf_entry.Subindex, /* UNS8 subindex*/

   244

                   0, /* UNS8 dataType*/

   245

                   CheckSDOAndContinue,/* Callback*/

   246

                   0); /* no block mode */

   247

    if(Ret)

   248

        MSG_ERR(0x1A03,"Erreur readNetworkDictCallbackAI",Ret);

   249

    return 1;

   250

}

   251

   252

void SaveNode(CO_Data* d, UNS8 nodeId)

   253

{

   254

    UNS8 Ret;

   255

    UNS32 data=0x65766173;

   256

    Ret = writeNetworkDictCallBackAI(d, /* CO_Data* d*/

   257

                    nodeId, /* UNS8 nodeId*/

   258

                    0x1010, /* UNS16 index*/

   259

                    1, /* UNS8 subindex*/

   260

                    4, /* UNS8 count*/

   261

                    0, /* UNS8 dataType*/

   262

                    (void *)&data,/* void *data*/

   263

                    CheckSDOAndContinue,/* Callback*/

   264

                    0,   /* no endianize*/

   265

                    0); /* no block mode */

   266

    if(Ret)

   267

        MSG_ERR(0x1A04,"Erreur writeNetworkDictCallBackAI",Ret);

   268

}