1

/******************************************************************************

     2

 *

     3

 *  m a s t e r . c

     4

 *

     5

 *  Methoden für einen EtherCAT-Master.

     6

 *

     7

 *  $Id$

     8

 *

     9

 *****************************************************************************/

    10

    11

#include <linux/module.h>

    12

#include <linux/kernel.h>

    13

#include <linux/string.h>

    14

#include <linux/slab.h>

    15

#include <linux/delay.h>

    16

    17

#include "../include/EtherCAT_rt.h"

    18

#include "../include/EtherCAT_si.h"

    19

#include "globals.h"

    20

#include "master.h"

    21

#include "slave.h"

    22

#include "types.h"

    23

#include "device.h"

    24

#include "command.h"

    25

    26

/*****************************************************************************/

    27

    28

/**

    29

   Konstruktor des EtherCAT-Masters.

    30

*/

    31

    32

void ec_master_init(ec_master_t *master /**< EtherCAT-Master */)

    33

{

    34

    master->slaves = NULL;

    35

    master->device = NULL;

    36

    37

    INIT_LIST_HEAD(&master->commands);

    38

    INIT_LIST_HEAD(&master->domains);

    39

    40

    ec_master_reset(master);

    41

}

    42

    43

/*****************************************************************************/

    44

    45

/**

    46

   Destruktor eines EtherCAT-Masters.

    47

    48

   Entfernt alle Kommandos aus der Liste, löscht den Zeiger

    49

   auf das Slave-Array und gibt die Prozessdaten frei.

    50

*/

    51

    52

void ec_master_clear(ec_master_t *master /**< EtherCAT-Master */)

    53

{

    54

    ec_master_reset(master);

    55

    56

    if (master->device) {

    57

        ec_device_clear(master->device);

    58

        kfree(master->device);

    59

    }

    60

}

    61

    62

/*****************************************************************************/

    63

    64

/**

    65

   Setzt den Master zurück in den Ausgangszustand.

    66

    67

   Bei einem "release" sollte immer diese Funktion aufgerufen werden,

    68

   da sonst Slave-Liste, Domains, etc. weiter existieren.

    69

*/

    70

    71

void ec_master_reset(ec_master_t *master

    72

                     /**< Zeiger auf den zurückzusetzenden Master */

    73

                     )

    74

{

    75

    unsigned int i;

    76

    ec_command_t *command, *next_command;

    77

    ec_domain_t *domain, *next_domain;

    78

    79

    // Alle Slaves entfernen

    80

    if (master->slaves) {

    81

        for (i = 0; i < master->slave_count; i++) {

    82

            ec_slave_clear(master->slaves + i);

    83

        }

    84

        kfree(master->slaves);

    85

        master->slaves = NULL;

    86

    }

    87

    master->slave_count = 0;

    88

    89

    // Kommando-Warteschlange leeren

    90

    list_for_each_entry_safe(command, next_command, &master->commands, list) {

    91

        command->state = EC_CMD_ERROR;

    92

        list_del_init(&command->list);

    93

    }

    94

    95

    // Domain-Liste leeren

    96

    list_for_each_entry_safe(domain, next_domain, &master->domains, list) {

    97

        list_del(&domain->list);

    98

        ec_domain_clear(domain);

    99

        kfree(domain);

   100

    }

   101

   102

    master->command_index = 0;

   103

    master->debug_level = 0;

   104

    master->stats.timeouts = 0;

   105

    master->stats.delayed = 0;

   106

    master->stats.corrupted = 0;

   107

    master->stats.unmatched = 0;

   108

    master->stats.t_last = 0;

   109

}

   110

   111

/*****************************************************************************/

   112

   113

/**

   114

   Öffnet das EtherCAT-Geraet des Masters.

   115

   116

   \return 0 wenn alles ok, < 0 wenn kein Gerät registriert wurde oder

   117

           es nicht geoeffnet werden konnte.

   118

*/

   119

   120

int ec_master_open(ec_master_t *master /**< Der EtherCAT-Master */)

   121

{

   122

    if (!master->device) {

   123

        EC_ERR("No device registered!\n");

   124

        return -1;

   125

    }

   126

   127

    if (ec_device_open(master->device)) {

   128

        EC_ERR("Could not open device!\n");

   129

        return -1;

   130

    }

   131

   132

    return 0;

   133

}

   134

   135

/*****************************************************************************/

   136

   137

/**

   138

   Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet.

   139

*/

   140

   141

void ec_master_close(ec_master_t *master /**< EtherCAT-Master */)

   142

{

   143

    if (!master->device) {

   144

        EC_WARN("Warning - Trying to close an unregistered device!\n");

   145

        return;

   146

    }

   147

   148

    if (ec_device_close(master->device))

   149

        EC_WARN("Warning - Could not close device!\n");

   150

}

   151

   152

/*****************************************************************************/

   153

   154

/**

   155

   Stellt ein Kommando in die Warteschlange.

   156

*/

   157

   158

void ec_master_queue_command(ec_master_t *master, /**< EtherCAT-Master */

   159

                             ec_command_t *command /**< Kommando */

   160

                             )

   161

{

   162

    ec_command_t *queued_command;

   163

   164

    // Ist das Kommando schon in der Warteschlange?

   165

    list_for_each_entry(queued_command, &master->commands, list) {

   166

        if (queued_command == command) {

   167

            command->state = EC_CMD_QUEUED;

   168

            if (unlikely(master->debug_level))

   169

                EC_WARN("command already queued.\n");

   170

            return;

   171

        }

   172

    }

   173

   174

    list_add_tail(&command->list, &master->commands);

   175

    command->state = EC_CMD_QUEUED;

   176

}

   177

   178

/*****************************************************************************/

   179

   180

/**

   181

   Sendet die Kommandos in der Warteschlange.

   182

   183

   \return 0 bei Erfolg, sonst < 0

   184

*/

   185

   186

void ec_master_send_commands(ec_master_t *master /**< EtherCAT-Master */)

   187

{

   188

    ec_command_t *command;

   189

    size_t command_size;

   190

    uint8_t *frame_data, *cur_data;

   191

    void *follows_word;

   192

    cycles_t start = 0, end;

   193

   194

    if (unlikely(master->debug_level > 0)) {

   195

        EC_DBG("ec_master_send\n");

   196

        start = get_cycles();

   197

    }

   198

   199

    // Zeiger auf Socket-Buffer holen

   200

    frame_data = ec_device_tx_data(master->device);

   201

    cur_data = frame_data + EC_FRAME_HEADER_SIZE;

   202

    follows_word = NULL;

   203

   204

    // Aktuellen Frame mit Kommandos füllen

   205

    list_for_each_entry(command, &master->commands, list) {

   206

        if (command->state != EC_CMD_QUEUED) continue;

   207

   208

        // Passt das aktuelle Kommando noch in den aktuellen Rahmen?

   209

        command_size = EC_COMMAND_HEADER_SIZE + command->data_size

   210

            + EC_COMMAND_FOOTER_SIZE;

   211

        if (cur_data - frame_data + command_size > EC_MAX_FRAME_SIZE) break;

   212

   213

        command->state = EC_CMD_SENT;

   214

        command->index = master->command_index++;

   215

   216

        if (unlikely(master->debug_level > 0))

   217

            EC_DBG("adding command 0x%02X\n", command->index);

   218

   219

        // Command-Following-Flag im letzten Kommando setzen

   220

        if (follows_word)

   221

            EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);

   222

   223

        // EtherCAT command header

   224

        EC_WRITE_U8 (cur_data,     command->type);

   225

        EC_WRITE_U8 (cur_data + 1, command->index);

   226

        EC_WRITE_U32(cur_data + 2, command->address.logical);

   227

        EC_WRITE_U16(cur_data + 6, command->data_size & 0x7FF);

   228

        EC_WRITE_U16(cur_data + 8, 0x0000);

   229

        follows_word = cur_data + 6;

   230

        cur_data += EC_COMMAND_HEADER_SIZE;

   231

   232

        // EtherCAT command data

   233

        memcpy(cur_data, command->data, command->data_size);

   234

        cur_data += command->data_size;

   235

   236

        // EtherCAT command footer

   237

        EC_WRITE_U16(cur_data, command->working_counter);

   238

        cur_data += EC_COMMAND_FOOTER_SIZE;

   239

    }

   240

   241

    if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {

   242

        if (unlikely(master->debug_level > 0)) EC_DBG("nothing to send.\n");

   243

        return;

   244

    }

   245

   246

    // EtherCAT frame header

   247

    EC_WRITE_U16(frame_data, ((cur_data - frame_data

   248

                               - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);

   249

   250

    // Rahmen auffüllen

   251

    while (cur_data - frame_data < EC_MIN_FRAME_SIZE)

   252

        EC_WRITE_U8(cur_data++, 0x00);

   253

   254

    if (unlikely(master->debug_level > 0))

   255

        EC_DBG("Frame size: %i\n", cur_data - frame_data);

   256

   257

    // Send frame

   258

    ec_device_send(master->device, cur_data - frame_data);

   259

   260

    if (unlikely(master->debug_level > 0)) {

   261

        end = get_cycles();

   262

        EC_DBG("ec_master_send finished in %ius.\n",

   263

               (u32) (end - start) * 1000 / cpu_khz);

   264

    }

   265

}

   266

   267

/*****************************************************************************/

   268

   269

/**

   270

   Wertet einen empfangenen Rahmen aus.

   271

   272

   \return 0 bei Erfolg, sonst < 0

   273

*/

   274

   275

void ec_master_receive(ec_master_t *master, /**< EtherCAT-Master */

   276

                      const uint8_t *frame_data, /**< Empfangene Daten */

   277

                      size_t size /**< Anzahl empfangene Datenbytes */

   278

                      )

   279

{

   280

    size_t frame_size, data_size;

   281

    uint8_t command_type, command_index;

   282

    unsigned int cmd_follows, matched;

   283

    const uint8_t *cur_data;

   284

    ec_command_t *command;

   285

   286

    if (unlikely(size < EC_FRAME_HEADER_SIZE)) {

   287

        master->stats.corrupted++;

   288

        ec_master_output_stats(master);

   289

        return;

   290

    }

   291

   292

    cur_data = frame_data;

   293

   294

    // Länge des gesamten Frames prüfen

   295

    frame_size = EC_READ_U16(cur_data) & 0x07FF;

   296

    cur_data += EC_FRAME_HEADER_SIZE;

   297

   298

    if (unlikely(frame_size > size)) {

   299

        master->stats.corrupted++;

   300

        ec_master_output_stats(master);

   301

        return;

   302

    }

   303

   304

    cmd_follows = 1;

   305

    while (cmd_follows) {

   306

        // Kommando-Header auswerten

   307

        command_type  = EC_READ_U8 (cur_data);

   308

        command_index = EC_READ_U8 (cur_data + 1);

   309

        data_size     = EC_READ_U16(cur_data + 6) & 0x07FF;

   310

        cmd_follows   = EC_READ_U16(cur_data + 6) & 0x8000;

   311

        cur_data += EC_COMMAND_HEADER_SIZE;

   312

   313

        if (unlikely(cur_data - frame_data

   314

                     + data_size + EC_COMMAND_FOOTER_SIZE > size)) {

   315

            master->stats.corrupted++;

   316

            ec_master_output_stats(master);

   317

            return;

   318

        }

   319

   320

        // Suche passendes Kommando in der Liste

   321

        matched = 0;

   322

        list_for_each_entry(command, &master->commands, list) {

   323

            if (command->state == EC_CMD_SENT

   324

                && command->type == command_type

   325

                && command->index == command_index

   326

                && command->data_size == data_size) {

   327

                matched = 1;

   328

                break;

   329

            }

   330

        }

   331

   332

        // Kein passendes Kommando in der Liste gefunden

   333

        if (!matched) {

   334

            master->stats.unmatched++;

   335

            ec_master_output_stats(master);

   336

            cur_data += data_size + EC_COMMAND_FOOTER_SIZE;

   337

            continue;

   338

        }

   339

   340

        // Empfangene Daten in Kommando-Datenspeicher kopieren

   341

        memcpy(command->data, cur_data, data_size);

   342

        cur_data += data_size;

   343

   344

        // Working-Counter setzen

   345

        command->working_counter = EC_READ_U16(cur_data);

   346

        cur_data += EC_COMMAND_FOOTER_SIZE;

   347

   348

        // Kommando aus der Liste entfernen

   349

        command->state = EC_CMD_RECEIVED;

   350

        list_del_init(&command->list);

   351

    }

   352

}

   353

   354

/*****************************************************************************/

   355

   356

/**

   357

   Sendet ein einzelnes Kommando und wartet auf den Empfang.

   358

   359

   Wenn der Slave nicht antwortet, wird das Kommando

   360

   nochmals gesendet.

   361

   362

   \return 0 bei Erfolg, sonst < 0

   363

*/

   364

   365

int ec_master_simple_io(ec_master_t *master, /**< EtherCAT-Master */

   366

                        ec_command_t *command /**< Kommando */

   367

                        )

   368

{

   369

    unsigned int response_tries_left;

   370

   371

    response_tries_left = 10;

   372

    do

   373

    {

   374

        ec_master_queue_command(master, command);

   375

        EtherCAT_rt_master_xio(master);

   376

   377

        if (command->state == EC_CMD_RECEIVED) {

   378

            break;

   379

        }

   380

        else if (command->state == EC_CMD_TIMEOUT) {

   381

            EC_ERR("Simple IO TIMED OUT!\n");

   382

            return -1;

   383

        }

   384

        else if (command->state == EC_CMD_ERROR) {

   385

            EC_ERR("Simple IO command error!\n");

   386

            return -1;

   387

        }

   388

    }

   389

    while (unlikely(!command->working_counter && --response_tries_left));

   390

   391

    if (unlikely(!response_tries_left)) {

   392

        EC_ERR("No response in simple IO!\n");

   393

        return -1;

   394

    }

   395

   396

    return 0;

   397

}

   398

   399

/*****************************************************************************/

   400

   401

/**

   402

   Durchsucht den EtherCAT-Bus nach Slaves.

   403

   404

   Erstellt ein Array mit allen Slave-Informationen die für den

   405

   weiteren Betrieb notwendig sind.

   406

   407

   \return 0 bei Erfolg, sonst < 0

   408

*/

   409

   410

int ec_master_bus_scan(ec_master_t *master /**< EtherCAT-Master */)

   411

{

   412

    ec_command_t command;

   413

    ec_slave_t *slave;

   414

    ec_slave_ident_t *ident;

   415

    unsigned int i;

   416

    uint8_t data[2];

   417

   418

    if (master->slaves || master->slave_count) {

   419

        EC_ERR("Slave scan already done!\n");

   420

        return -1;

   421

    }

   422

   423

    // Determine number of slaves on bus

   424

    ec_command_init_brd(&command, 0x0000, 4);

   425

    if (unlikely(ec_master_simple_io(master, &command))) return -1;

   426

    master->slave_count = command.working_counter;

   427

    EC_INFO("Found %i slaves on bus.\n", master->slave_count);

   428

   429

    if (!master->slave_count) return 0;

   430

   431

    if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count

   432

                                                  * sizeof(ec_slave_t),

   433

                                                  GFP_KERNEL))) {

   434

        EC_ERR("Could not allocate memory for slaves!\n");

   435

        return -1;

   436

    }

   437

   438

    // Init slaves

   439

    for (i = 0; i < master->slave_count; i++) {

   440

        slave = master->slaves + i;

   441

        ec_slave_init(slave, master);

   442

        slave->ring_position = i;

   443

        slave->station_address = i + 1;

   444

    }

   445

   446

    // For every slave in the list

   447

    for (i = 0; i < master->slave_count; i++)

   448

    {

   449

        slave = master->slaves + i;

   450

   451

        // Write station address

   452

        EC_WRITE_U16(data, slave->station_address);

   453

        ec_command_init_apwr(&command, slave->ring_position,

   454

                             0x0010, sizeof(uint16_t), data);

   455

        if (unlikely(ec_master_simple_io(master, &command))) {

   456

            EC_ERR("Writing station address failed on slave %i!\n", i);

   457

            return -1;

   458

        }

   459

   460

        // Fetch all slave information

   461

        if (ec_slave_fetch(slave)) return -1;

   462

   463

        // Search for identification in "database"

   464

        ident = slave_idents;

   465

        while (ident) {

   466

            if (unlikely(ident->vendor_id == slave->sii_vendor_id

   467

                         && ident->product_code == slave->sii_product_code)) {

   468

                slave->type = ident->type;

   469

                break;

   470

            }

   471

            ident++;

   472

        }

   473

   474

        if (!slave->type)

   475

            EC_WARN("Unknown slave device (vendor 0x%08X, code 0x%08X) at"

   476

                    " position %i.\n", slave->sii_vendor_id,

   477

                    slave->sii_product_code, i);

   478

    }

   479

   480

    return 0;

   481

}

   482

   483

/*****************************************************************************/

   484

   485

/**

   486

   Statistik-Ausgaben während des zyklischen Betriebs.

   487

   488

   Diese Funktion sorgt dafür, dass Statistiken während des zyklischen

   489

   Betriebs bei Bedarf, aber nicht zu oft ausgegeben werden.

   490

   491

   Die Ausgabe erfolgt gesammelt höchstens einmal pro Sekunde.

   492

*/

   493