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/ecrt.h"

    18

#include "globals.h"

    19

#include "master.h"

    20

#include "slave.h"

    21

#include "types.h"

    22

#include "device.h"

    23

#include "command.h"

    24

#include "ethernet.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->command_queue);

    38

    INIT_LIST_HEAD(&master->domains);

    39

    INIT_LIST_HEAD(&master->eoe_slaves);

    40

    41

    ec_command_init(&master->simple_command);

    42

    ec_command_init(&master->watch_command);

    43

    44

    ec_master_reset(master);

    45

}

    46

    47

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

    48

    49

/**

    50

   Destruktor eines EtherCAT-Masters.

    51

    52

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

    53

   auf das Slave-Array und gibt die Prozessdaten frei.

    54

*/

    55

    56

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

    57

{

    58

    ec_master_reset(master);

    59

    60

    if (master->device) {

    61

        ec_device_clear(master->device);

    62

        kfree(master->device);

    63

    }

    64

    65

    ec_command_clear(&master->simple_command);

    66

    ec_command_clear(&master->watch_command);

    67

}

    68

    69

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

    70

    71

/**

    72

   Setzt den Master zurück in den Ausgangszustand.

    73

    74

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

    75

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

    76

*/

    77

    78

void ec_master_reset(ec_master_t *master

    79

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

    80

                     )

    81

{

    82

    unsigned int i;

    83

    ec_command_t *command, *next_c;

    84

    ec_domain_t *domain, *next_d;

    85

    ec_eoe_t *eoe, *next_eoe;

    86

    87

    // Alle Slaves entfernen

    88

    if (master->slaves) {

    89

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

    90

            ec_slave_clear(master->slaves + i);

    91

        }

    92

        kfree(master->slaves);

    93

        master->slaves = NULL;

    94

    }

    95

    master->slave_count = 0;

    96

    97

    // Kommando-Warteschlange leeren

    98

    list_for_each_entry_safe(command, next_c, &master->command_queue, queue) {

    99

        command->state = EC_CMD_ERROR;

   100

        list_del_init(&command->queue);

   101

    }

   102

   103

    // Domain-Liste leeren

   104

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

   105

        list_del(&domain->list);

   106

        ec_domain_clear(domain);

   107

        kfree(domain);

   108

    }

   109

   110

    // EOE-Liste leeren

   111

    list_for_each_entry_safe(eoe, next_eoe, &master->domains, list) {

   112

        list_del(&eoe->list);

   113

        ec_eoe_clear(eoe);

   114

        kfree(eoe);

   115

    }

   116

   117

    master->command_index = 0;

   118

    master->debug_level = 0;

   119

    master->timeout = 100; // us

   120

   121

    master->slaves_responding = 0;

   122

    master->slave_states = EC_SLAVE_STATE_UNKNOWN;

   123

   124

    master->stats.timeouts = 0;

   125

    master->stats.delayed = 0;

   126

    master->stats.corrupted = 0;

   127

    master->stats.unmatched = 0;

   128

    master->stats.t_last = 0;

   129

}

   130

   131

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

   132

   133

/**

   134

   Stellt ein Kommando in die Warteschlange.

   135

*/

   136

   137

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

   138

                             ec_command_t *command /**< Kommando */

   139

                             )

   140

{

   141

    ec_command_t *queued_command;

   142

   143

    // Ist das Kommando schon in der Warteschlange?

   144

    list_for_each_entry(queued_command, &master->command_queue, queue) {

   145

        if (queued_command == command) {

   146

            command->state = EC_CMD_QUEUED;

   147

            if (unlikely(master->debug_level))

   148

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

   149

            return;

   150

        }

   151

    }

   152

   153

    list_add_tail(&command->queue, &master->command_queue);

   154

    command->state = EC_CMD_QUEUED;

   155

}

   156

   157

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

   158

   159

/**

   160

   Sendet die Kommandos in der Warteschlange.

   161

   162

   \return 0 bei Erfolg, sonst < 0

   163

*/

   164

   165

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

   166

{

   167

    ec_command_t *command;

   168

    size_t command_size;

   169

    uint8_t *frame_data, *cur_data;

   170

    void *follows_word;

   171

    cycles_t start = 0, end;

   172

   173

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

   174

        EC_DBG("ec_master_send\n");

   175

        start = get_cycles();

   176

    }

   177

   178

    // Zeiger auf Socket-Buffer holen

   179

    frame_data = ec_device_tx_data(master->device);

   180

    cur_data = frame_data + EC_FRAME_HEADER_SIZE;

   181

    follows_word = NULL;

   182

   183

    // Aktuellen Frame mit Kommandos füllen

   184

    list_for_each_entry(command, &master->command_queue, queue) {

   185

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

   186

   187

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

   188

        command_size = EC_COMMAND_HEADER_SIZE + command->data_size

   189

            + EC_COMMAND_FOOTER_SIZE;

   190

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

   191

   192

        command->state = EC_CMD_SENT;

   193

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

   194

   195

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

   196

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

   197

   198

        // Command-Following-Flag im letzten Kommando setzen

   199

        if (follows_word)

   200

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

   201

   202

        // EtherCAT command header

   203

        EC_WRITE_U8 (cur_data,     command->type);

   204

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

   205

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

   206

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

   207

        EC_WRITE_U16(cur_data + 8, 0x0000);

   208

        follows_word = cur_data + 6;

   209

        cur_data += EC_COMMAND_HEADER_SIZE;

   210

   211

        // EtherCAT command data

   212

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

   213

        cur_data += command->data_size;

   214

   215

        // EtherCAT command footer

   216

        EC_WRITE_U16(cur_data, 0x0000); // Working counter

   217

        cur_data += EC_COMMAND_FOOTER_SIZE;

   218

    }

   219

   220

    if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {

   221

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

   222

        return;

   223

    }

   224

   225

    // EtherCAT frame header

   226

    EC_WRITE_U16(frame_data, ((cur_data - frame_data

   227

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

   228

   229

    // Rahmen auffüllen

   230

    while (cur_data - frame_data < EC_MIN_FRAME_SIZE)

   231

        EC_WRITE_U8(cur_data++, 0x00);

   232

   233

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

   234

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

   235

   236

    // Send frame

   237

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

   238

   239

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

   240

        end = get_cycles();

   241

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

   242

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

   243

    }

   244

}

   245

   246

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

   247

   248

/**

   249

   Wertet einen empfangenen Rahmen aus.

   250

   251

   \return 0 bei Erfolg, sonst < 0

   252

*/

   253

   254

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

   255

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

   256

                      size_t size /**< Anzahl empfangene Datenbytes */

   257

                      )

   258

{

   259

    size_t frame_size, data_size;

   260

    uint8_t command_type, command_index;

   261

    unsigned int cmd_follows, matched;

   262

    const uint8_t *cur_data;

   263

    ec_command_t *command;

   264

   265

    if (unlikely(size < EC_FRAME_HEADER_SIZE)) {

   266

        master->stats.corrupted++;

   267

        ec_master_output_stats(master);

   268

        return;

   269

    }

   270

   271

    cur_data = frame_data;

   272

   273

    // Länge des gesamten Frames prüfen

   274

    frame_size = EC_READ_U16(cur_data) & 0x07FF;

   275

    cur_data += EC_FRAME_HEADER_SIZE;

   276

   277

    if (unlikely(frame_size > size)) {

   278

        master->stats.corrupted++;

   279

        ec_master_output_stats(master);

   280

        return;

   281

    }

   282

   283

    cmd_follows = 1;

   284

    while (cmd_follows) {

   285

        // Kommando-Header auswerten

   286

        command_type  = EC_READ_U8 (cur_data);

   287

        command_index = EC_READ_U8 (cur_data + 1);

   288

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

   289

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

   290

        cur_data += EC_COMMAND_HEADER_SIZE;

   291

   292

        if (unlikely(cur_data - frame_data

   293

                     + data_size + EC_COMMAND_FOOTER_SIZE > size)) {

   294

            master->stats.corrupted++;

   295

            ec_master_output_stats(master);

   296

            return;

   297

        }

   298

   299

        // Suche passendes Kommando in der Liste

   300

        matched = 0;

   301

        list_for_each_entry(command, &master->command_queue, queue) {

   302

            if (command->state == EC_CMD_SENT

   303

                && command->type == command_type

   304

                && command->index == command_index

   305

                && command->data_size == data_size) {

   306

                matched = 1;

   307

                break;

   308

            }

   309

        }

   310

   311

        // Kein passendes Kommando in der Liste gefunden

   312

        if (!matched) {

   313

            master->stats.unmatched++;

   314

            ec_master_output_stats(master);

   315

            cur_data += data_size + EC_COMMAND_FOOTER_SIZE;

   316

            continue;

   317

        }

   318

   319

        // Empfangene Daten in Kommando-Datenspeicher kopieren

   320

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

   321

        cur_data += data_size;

   322

   323

        // Working-Counter setzen

   324

        command->working_counter = EC_READ_U16(cur_data);

   325

        cur_data += EC_COMMAND_FOOTER_SIZE;

   326

   327

        // Kommando aus der Warteschlange entfernen

   328

        command->state = EC_CMD_RECEIVED;

   329

        list_del_init(&command->queue);

   330

    }

   331

}

   332

   333

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

   334

   335

/**

   336

   Sendet ein einzelnes Kommando und wartet auf den Empfang.

   337

   338

   Wenn der Slave nicht antwortet, wird das Kommando

   339

   nochmals gesendet.

   340

   341

   \return 0 bei Erfolg, sonst < 0

   342

*/

   343

   344

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

   345

                        ec_command_t *command /**< Kommando */

   346

                        )

   347

{

   348

    unsigned int response_tries_left;

   349

   350

    response_tries_left = 10;

   351

   352

    while (1)

   353

    {

   354

        ec_master_queue_command(master, command);

   355

        ecrt_master_sync_io(master);

   356

   357

        if (command->state == EC_CMD_RECEIVED) {

   358

            if (likely(command->working_counter))

   359

                return 0;

   360

        }

   361

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

   362

            EC_ERR("Simple-IO TIMEOUT!\n");

   363

            return -1;

   364

        }

   365

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

   366

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

   367

            return -1;

   368

        }

   369

   370

        // Keine direkte Antwort. Dem Slave Zeit lassen...

   371

        udelay(100);

   372

   373

        if (unlikely(--response_tries_left)) {

   374

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

   375

            return -1;

   376

        }

   377

    }

   378

}

   379

   380

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

   381

   382

/**

   383

   Durchsucht den EtherCAT-Bus nach Slaves.

   384

   385

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

   386

   weiteren Betrieb notwendig sind.

   387

   388

   \return 0 bei Erfolg, sonst < 0

   389

*/

   390

   391

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

   392

{

   393

    ec_slave_t *slave;

   394

    ec_slave_ident_t *ident;

   395

    unsigned int i;

   396

    ec_command_t *command;

   397

    ec_eoe_t *eoe;

   398

   399

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

   400

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

   401

        return -1;

   402

    }

   403

   404

    command = &master->simple_command;

   405

   406

    // Determine number of slaves on bus

   407

    if (ec_command_brd(command, 0x0000, 4)) return -1;

   408

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

   409

    master->slave_count = command->working_counter;

   410

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

   411

   412

    if (!master->slave_count) return 0;

   413

   414

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

   415

                                                  * sizeof(ec_slave_t),

   416

                                                  GFP_KERNEL))) {

   417

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

   418

        return -1;

   419

    }

   420

   421

    // Init slaves

   422

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

   423

        slave = master->slaves + i;

   424

        ec_slave_init(slave, master);

   425

        slave->ring_position = i;

   426

        slave->station_address = i + 1;

   427

    }

   428

   429

    // For every slave in the list

   430

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

   431

    {

   432

        slave = master->slaves + i;

   433

   434

        // Write station address

   435

        if (ec_command_apwr(command, slave->ring_position,

   436

                            0x0010, sizeof(uint16_t))) return -1;

   437

        EC_WRITE_U16(command->data, slave->station_address);

   438

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

   439

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

   440

            return -1;

   441

        }

   442

   443

        // Fetch all slave information

   444

        if (ec_slave_fetch(slave)) return -1;

   445

   446

        // Search for identification in "database"

   447

        ident = slave_idents;

   448

        while (ident->type) {

   449

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

   450

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

   451

                slave->type = ident->type;

   452

                break;

   453

            }

   454

            ident++;

   455

        }

   456

   457

        if (!slave->type)

   458

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

   459

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

   460

                    slave->sii_product_code, i);

   461

   462

        // Does the slave support EoE?

   463

        if (slave->sii_mailbox_protocols & EC_MBOX_EOE) {

   464

            if (!(eoe = kmalloc(sizeof(ec_eoe_t), GFP_KERNEL))) {

   465

                EC_ERR("Failed to allocate memory for EoE-Object.\n");

   466

                return -1;

   467

            }

   468

   469

            ec_eoe_init(eoe, slave);

   470

            list_add_tail(&eoe->list, &master->eoe_slaves);

   471

        }

   472

    }

   473

   474

    return 0;

   475

}

   476

   477

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

   478

   479

/**

   480

   Statistik-Ausgaben während des zyklischen Betriebs.

   481

   482

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

   483

   Betriebs bei Bedarf, aber nicht zu oft ausgegeben werden.

   484

   485

   Die Ausgabe erfolgt gesammelt höchstens einmal pro Sekunde.

   486

*/

   487

   488

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

   489

{

   490

    cycles_t t_now = get_cycles();