src/lss.c

   1 /*
   2 This file is part of CanFestival, a library implementing CanOpen Stack.
   3
   4  Author: CANopen Canada (canfestival@canopencanada.ca)
   5
   6 See COPYING file for copyrights details.
   7
   8 This library is free software; you can redistribute it and/or
   9 modify it under the terms of the GNU Lesser General Public
  10 License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version.
  12
  13 This library is distributed in the hope that it will be useful,
  14 but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 Lesser General Public License for more details.
  17
  18 You should have received a copy of the GNU Lesser General Public
  19 License along with this library; if not, write to the Free Software
  20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 */
  22
  23 #include <applicfg.h>
  24 #include <def.h>
  25
  26 #include <can.h>
  27 #include <data.h>
  28 #include <objdictdef.h>
  29 #include <objacces.h>
  30 #include "can_driver.h"
  31
  32 #ifdef LED_ENABLE
  33 #include "led.h"
  34 #else
  35 #define led_set_state(a,b)
  36 #endif
  37
  38 #include "lss.h"
  39
  40
  41
  42 /*
  43         NOTES
  44
  45         1. since in the LSS protocol all LSS Slave use the same COB, only 1 Slave
  46         must be allowed to communicate with the Master
  47
  48         2. the Master always take the iniative. the Slave is only allowed to transmit
  49         within a confirmed service
  50
  51         3. requesting message (from the Master) using COB-ID 2021 and response messages
  52         (from the Slave) using COB-ID 2020
  53 */
  54
  55 /*
  56         0 = this slave is not talking to the master
  57         1 = this slave is talking to the master (this slave has been selected via )
  58 */
  59 int     slave_selector;
  60
  61 int     current_mode;
  62
  63 int     lss_table_selector, lss_table_index;
  64
  65
  66 /* slave storing the information sent by the master */
  67 UNS32 lss_buffer[10];
  68 /*
  69         how this buffer is used
  70
  71         for a SLAVE
  72         [0..3]  used to store the LSS Address
  73         [4..9]  use by LSS Identify Remort Slave
  74
  75         for the MASTER
  76         [0..3] hold the answer from the slave regarding its ID
  77 */
  78
  79
  80 void lss_copy(UNS8 *data, UNS32 value)
  81 /* transfert 32 bits value into uns8 data vector */
  82 {
  83         data[0] = value & 0xff;
  84         data[1] = (value>>8) & 0xff;
  85         data[2] = (value>>16) & 0xff;
  86         data[3] = (value>>24) & 0xff;
  87 }
  88
  89
  90 UNS32 lss_get_value(UNS8 *data)
  91 /* build a 'UNS32' value from a 'unsigned char' vector */
  92 {
  93         return data[0] + (data[1]<<8) + (data[2]<<16) + (data[3]<<24);
  94 }
  95
  96
  97 void lss_init_msg(Message *msg)
  98 {
  99         msg->cob_id.w = 0;
 100         msg->rtr = 0;
 101         msg->len = 0;
 102         msg->data[0] = 0;
 103         msg->data[1] = 0;
 104         msg->data[2] = 0;
 105         msg->data[3] = 0;
 106         msg->data[4] = 0;
 107         msg->data[5] = 0;
 108         msg->data[6] = 0;
 109         msg->data[7] = 0;
 110 }
 111
 112
 113 void lss_SwitchModeGlobal(CO_Data *d, UNS32 mode)
 114 /*
 115         this service is used to switch all LSS slaves in the network between operation
 116         mode and configuration mode.
 117 */
 118 {
 119         Message msg;
 120         lss_init_msg(&msg);
 121
 122         /*
 123                 sending a COB-ID 2021
 124                 [0] = 4 (for switch mode global)
 125                 [1] = 0 for operation mode, = 1 for configuration mode
 126                 [2..7] = 0 reserved
 127         */
 128
 129         if (!(d->iam_a_slave))
 130         {
 131                 msg.cob_id.w = 0x07E5 /* 2021 */;
 132
 133                 msg.len = 2;
 134                 msg.data[0] = 4;
 135                 msg.data[1] = (UNS8)mode;
 136
 137                 /* transmit */
 138                 (*d->canSend)(&msg);
 139         }
 140         else
 141         {
 142                 /* set mode global */
 143                 current_mode = mode;
 144         }
 145 }
 146
 147
 148 void lss_SwitchModeSelective_master(CO_Data *d, UNS32 *LSSaddr)
 149 /*
 150         LSS address : <vendor-id> <product-code> <revision-number> <serial-number>
 151         vendor-id : provided by CiA
 152         identical to the CANopen identity object
 153
 154         select the slave corresponding to this ADDRESS
 155 */
 156 {
 157         Message msg;
 158         lss_init_msg(&msg);
 159
 160         if (d->iam_a_slave) /* not the master */
 161                 return;
 162
 163         msg.cob_id.w = 0x07E5 /* 2021 */;
 164         msg.len=5;
 165
 166         msg.data[0] = 64;
 167         lss_copy(msg.data+1, LSSaddr[0]);
 168         /* transmit */
 169         (*d->canSend)(&msg);
 170
 171         msg.data[0] = 65;
 172         lss_copy(msg.data+1, LSSaddr[1]);
 173         /* transmit */
 174         (*d->canSend)(&msg);
 175
 176         msg.data[0] = 66;
 177         lss_copy(msg.data+1, LSSaddr[2]);
 178         /* transmit */
 179         (*d->canSend)(&msg);
 180
 181         msg.data[0] = 67;
 182         lss_copy(msg.data+1, LSSaddr[3]);
 183         /* transmit */
 184         (*d->canSend)(&msg);
 185 }
 186
 187
 188 UNS32 lss_validate_address(CO_Data* d)
 189 {
 190 #if 0
 191         extern s_identity obj1018;
 192
 193         /* maybe we should go throught getODentry but those
 194         data are 1) RO and 2) the proper ID of this device */
 195 #else
 196         UNS32  r, vendor_id, product_code, revision_number, serial_number;
 197         UNS8   sz = sizeof(UNS32), dt = int32;
 198
 199         r = getODentry(d, 0x1018, 1, (void *)&vendor_id, &sz, &dt, 0);
 200         r = getODentry(d, 0x1018, 2, (void *)&product_code, &sz, &dt, 0);
 201         r = getODentry(d, 0x1018, 3, (void *)&revision_number, &sz, &dt, 0);
 202         r = getODentry(d, 0x1018, 4, (void *)&serial_number, &sz, &dt, 0);
 203
 204 #endif
 205         /*
 206                 if
 207                 lss_buffer[0] == vendor-id
 208                 lss_buffer[1] == product code
 209                 lss_buffer[2] == revision
 210                 lss_buffer[3] == serial
 211
 212                 then return 1
 213                 else return 0;
 214         */
 215
 216         if (lss_buffer[0] == vendor_id  &&
 217             lss_buffer[1] == product_code &&
 218             lss_buffer[2] == revision_number &&
 219             lss_buffer[3] == serial_number)
 220         {
 221                 return 1;
 222         }
 223
 224         return 0;
 225 }
 226
 227
 228 void lss_SwitchModeSelective_slave(CO_Data *d)
 229 /*
 230         SwitchModeSelective for the SLAVE
 231         received the frames from the master and start building
 232         the lss address
 233 */
 234 {
 235         Message msg;
 236         lss_init_msg(&msg);
 237
 238         /*
 239                 the master broadcast the address of a particular slave
 240                 for 64,65 and 66 store the partial address
 241                 when 67 arrive process the call and asknowledge if necessary
 242         */
 243
 244         if (lss_validate_address(d))
 245         {
 246                 /* slave should transmit cob_id 2020  */
 247                 msg.cob_id.w = 0x07E4 /* 2020 */;
 248
 249                 msg.len = 2;
 250                 msg.data[0] = 68;
 251                 msg.data[1] = (UNS8)current_mode;
 252
 253                 /* transmit */
 254                 (*d->canSend)(&msg);
 255         }
 256
 257         /* reset the address */
 258         lss_buffer[0] = 0;
 259         lss_buffer[1] = 0;
 260         lss_buffer[2] = 0;
 261         lss_buffer[3] = 0;
 262 }
 263
 264
 265 void lss_ConfigureNode_ID(CO_Data *d, UNS32 node_id)
 266 /*
 267         through this service the LSS Master configures the NMT-address
 268         parameter of a LSS slave.
 269 */
 270 {
 271         Message msg;
 272         lss_init_msg(&msg);
 273
 274         if (!(d->iam_a_slave))
 275         {
 276                 msg.cob_id.w = 0x07E5 /* 2021 */;
 277
 278                 msg.len = 2;
 279                 msg.data[0] = 17;
 280                 msg.data[1] = (UNS8)node_id;
 281
 282                 /* transmit */
 283                 (*d->canSend)(&msg);
 284         }
 285         else
 286         {
 287                 /*
 288                         receiving NODE ID from the master
 289                 */
 290
 291                 /*
 292                         error code
 293                         0 = success
 294                         1 = node id out of range
 295                         2..254 = reserved
 296                         255 = implementation specific error occured
 297                                 spec error = mode detailed error
 298                 */
 299                 msg.cob_id.w = 0x07E4 /* 2020 */;
 300
 301                 msg.len = 3;
 302                 msg.data[0] = 17;
 303                 /* msg.data[1] = error code */
 304                 /* msg.data[2] = spec error */
 305
 306                 /* transmit */
 307                 (*d->canSend)(&msg);
 308         }
 309 }
 310
 311
 312 void lss_ConfigureBitTimingParameters(CO_Data *d,
 313                                       UNS32 table_selector,
 314                                       UNS32 table_index)
 315 /*
 316         this service allows all LSS slaves in configuration mode.
 317         must be followed by an Activate Bit Timing Parameters
 318 */
 319 {
 320         Message msg;
 321         lss_init_msg(&msg);
 322
 323         if (!(d->iam_a_slave))
 324         {
 325                 msg.cob_id.w = 0x07E5 /* 2021 */;
 326
 327                 msg.len = 3;
 328                 msg.data[0] = 19;
 329                 msg.data[1] = (UNS8)table_selector;
 330                 msg.data[2] = (UNS8)table_index;
 331
 332                 /* transmit */
 333                 (*d->canSend)(&msg);
 334         }
 335         else
 336         {
 337                 UNS8 error_code;
 338
 339                 /* validate if this baudrate is possible */
 340                 if (table_selector == 0  &&  baudrate_valid(table_index) == 1)
 341                 {
 342                         lss_table_selector = table_selector;
 343                         lss_table_index = table_index;
 344                 }
 345                 else
 346                         error_code = 1; /* bit timing not supported */
 347
 348                 msg.cob_id.w = 0x07E4 /* 2020 */;
 349
 350                 msg.len = 3;
 351                 msg.data[0] = 19;
 352                 msg.data[1] = error_code;
 353                 msg.data[2] = 0;
 354
 355                 /* transmit */
 356                 (*d->canSend)(&msg);
 357         }
 358
 359         led_set_state(d, LED_AUTOBITRATE);
 360 }
 361
 362
 363 void lss_ActivateBitTimingParameters_master(CO_Data *d, unsigned short switch_delay)
 364 /*
 365         switch_delay in ms
 366
 367         switch_delay has to be longer than the longest timeof any node
 368         in the network to avoid that a node already switches while another
 369         stills transmist the old bit timing parameters
 370 */
 371 {
 372         Message msg;
 373         lss_init_msg(&msg);
 374
 375         if (d->iam_a_slave)
 376                 return;
 377
 378         msg.cob_id.w = 0x07E5 /* 2021 */;
 379
 380         msg.len = 3;
 381         msg.data[0] = 21;
 382         msg.data[1] = (UNS8)(switch_delay &  0xff);
 383         msg.data[2] = (UNS8)((switch_delay >> 8) & 0xff);
 384
 385 #ifdef LED_ENABLE
 386         led_set_state(LED_AUTOBITRATE);
 387 #endif
 388         /* transmit */
 389         (*d->canSend)(&msg);
 390 }
 391
 392
 393 void lss_ActivateBitTimingParameters_slave(UNS8 byte_low, UNS8 byte_high)
 394 {
 395         /* rebuild the delay value (short) from the 2 (UNS8) data */
 396         unsigned short switch_delay = byte_low + (((UNS16)byte_high)<<8);
 397
 398         /* set the baudrate to the value proposed by the master */
 399         if (lss_table_selector == 0)
 400                 baudrate_set(lss_table_index);
 401
 402         /* wait for switch_delay ms before continuing */
 403 }
 404
 405
 406 void lss_StoreConfiguredParameters(CO_Data *d)
 407 /*
 408         store configured parameters into non-volatile storage
 409 */
 410 {
 411         Message msg;
 412         lss_init_msg(&msg);
 413
 414         if (!(d->iam_a_slave))
 415         {
 416                 msg.cob_id.w = 0x07E5 /* 2021 */;
 417
 418                 msg.len = 1;
 419                 msg.data[0] = 23;
 420
 421                 /* transmit */
 422                 (*d->canSend)(&msg);
 423         }
 424         else
 425         {
 426                 msg.cob_id.w = 0x07E4 /* 2020 */;
 427
 428                 msg.data[0] = 23;
 429                 /* msg.data[1] = error code; */
 430                 /* msg.data[2] = spec err */
 431
 432                 /* transmit */
 433                 (*d->canSend)(&msg);
 434         }
 435 }
 436
 437
 438 void lss_InquireLSSAddress_master(CO_Data *d, int flag)
 439 /*
 440         this service allows to determine the LSS-address parameters of a LSS-slave in
 441         configuration mode
 442
 443         request 1 single item of the LSS address
 444         0 = request vendor-id
 445         1 = request product-id
 446         2 = request revision
 447         3 = request serial
 448 */
 449 {
 450         Message msg;
 451         lss_init_msg(&msg);
 452
 453         if (!(d->iam_a_slave))
 454         {
 455                 msg.cob_id.w = 0x07E5 /* 2021 */;
 456
 457                 msg.len = 1;
 458                 msg.data[0] = 90 + flag;
 459
 460                 /* transmit */
 461                 (*d->canSend)(&msg);
 462         }
 463 }
 464
 465
 466 int lss_InquireLSSAddress_slave(CO_Data *d, int cs)
 467 {
 468         Message msg;
 469         lss_init_msg(&msg);
 470
 471         if (!(d->iam_a_slave)) /* not a slave */
 472                 return -1;
 473
 474         UNS32 value = 0;
 475
 476         switch(cs)
 477         {
 478                 case 90:
 479                         value = 0; /* = vendor id */
 480                         break;
 481                 case 91:
 482                         value = 0; /* = product code */
 483                         break;
 484                 case 92:
 485                         value = 0; /* = revision */
 486                         break;
 487                 case 93:
 488                         value = 0; /* = serial */
 489                         break;
 490         }
 491
 492         if (value > 0)
 493         {
 494                 msg.cob_id.w = 0x07E4 /* 2020 */;
 495
 496                 msg.len=5;
 497                 msg.data[0] = cs;
 498                 lss_copy(msg.data+1, value);
 499
 500                 /* transmit */
 501                 (*d->canSend)(&msg);
 502
 503                 return 0;
 504         }
 505
 506         return -1;
 507 }
 508
 509
 510 void lss_IdentifyRemoteSlaves(CO_Data *d,
 511                               UNS32 vendor_id,
 512                               UNS32 product_code,
 513                               UNS32 rev_low,
 514                               UNS32 rev_high,
 515                               UNS32 serial_low,
 516                               UNS32 serial_high)
 517 /*
 518         throught this service, the LSS Master requests all LSS slave whose LSS address
 519         meets the LSSaddr_sel to idenfity themselves through LSS Identify Slave
 520 */
 521 {
 522         Message msg;
 523         lss_init_msg(&msg);
 524
 525         if (!(d->iam_a_slave))
 526         {
 527                 /*
 528                         request answers from all slaves corresponding
 529                         to the revision and serial range of values
 530                 */
 531
 532                 msg.cob_id.w = 0x07E5 /* 2021 */;
 533                 msg.len=5;
 534
 535                 msg.data[0] = 70;
 536                 lss_copy(msg.data+1, vendor_id);
 537                 /* transmit */
 538                 (*d->canSend)(&msg);
 539
 540                 msg.data[0] = 71;
 541                 lss_copy(msg.data+1, product_code);
 542                 /* transmit */
 543                 (*d->canSend)(&msg);
 544
 545                 msg.data[0] = 72; /* revision number low */
 546                 lss_copy(msg.data+1, rev_low);
 547                 /* transmit */
 548                 (*d->canSend)(&msg);
 549
 550                 msg.data[0] = 73; /* revision number high */
 551                 lss_copy(msg.data+1, rev_high);
 552                 /* transmit */
 553                 (*d->canSend)(&msg);
 554
 555                 msg.data[0] = 74; /* serial number low */
 556                 lss_copy(msg.data+1, serial_low);
 557                 /* transmit */
 558                 (*d->canSend)(&msg);
 559
 560                 msg.data[0] = 75; /* serial number high */
 561                 lss_copy(msg.data+1, serial_high);
 562                 /* transmit */
 563                 (*d->canSend)(&msg);
 564         }
 565 }
 566
 567
 568 int lss_validate_range_addr(CO_Data *d)
 569 {
 570         /*
 571                 if
 572
 573                 lss_buffer[4] == vendor_id
 574                 lss_buffer[5] == product code
 575                 lss_buffer[6] <= revision <= lss_buffer[7]
 576                 lss_buffer[8] <= serial <= lss_buffer[9]
 577
 578                 then return 1
 579                 else return 0
 580         */
 581
 582         UNS32  r, vendor_id, product_code, revision_number, serial_number;
 583         UNS8   sz = sizeof(UNS32), dt = int32;
 584
 585         r = getODentry(d, 0x1018, 1, (void *)&vendor_id, &sz, &dt, 0);
 586         r = getODentry(d, 0x1018, 2, (void *)&product_code, &sz, &dt, 0);
 587         r = getODentry(d, 0x1018, 3, (void *)&revision_number, &sz, &dt, 0);
 588         r = getODentry(d, 0x1018, 4, (void *)&serial_number, &sz, &dt, 0);
 589
 590         if (lss_buffer[4] == vendor_id  &&
 591             lss_buffer[5] == product_code &&
 592             lss_buffer[6] <= revision_number &&
 593             revision_number <= lss_buffer[7] &&
 594             lss_buffer[8] <= serial_number &&
 595             serial_number <= lss_buffer[9])
 596         {
 597                 return 1;
 598         }
 599
 600         return 0;
 601 }
 602
 603
 604 void lss_IdentifySlave(CO_Data *d)
 605 /*
 606         through this service, an LSS slave indicates that it is a slave
 607         with LSS address within the LSSaddr_sel
 608 */
 609 {
 610         Message msg;
 611         lss_init_msg(&msg);
 612
 613         if (lss_validate_range_addr(d))
 614         {
 615                 msg.cob_id.w = 0x07E4 /* 2020 */;
 616
 617                 msg.len = 1;
 618                 msg.data[0] = 79;
 619
 620                 /* transmit */
 621                 (*d->canSend)(&msg);
 622         }
 623
 624         /* reset */
 625         lss_buffer[4] = 0;
 626         lss_buffer[5] = 0;
 627         lss_buffer[6] = 0;
 628         lss_buffer[7] = 0;
 629         lss_buffer[8] = 0;
 630         lss_buffer[9] = 0;
 631 }
 632
 633
 634 int lss_process_msg(CO_Data *d, Message *msg)
 635 {
 636         /* process the incoming message */
 637         if (msg->cob_id.w == 0x07E4 /* 2020 */)
 638         {
 639                 // should be the master
 640                 // a slave just answered
 641                 switch(msg->data[0])
 642                 {
 643                         /* slave acknowledging the SwitchModeSelective call */
 644                         case 68:
 645                                 /* msg->data[1] contains the 'mode global' value from the slave*/
 646                                 break;
 647
 648                         /* a slave had acknowledged the call from LSS Identify Remote Slave */
 649                         case 79:
 650                                 break;
 651
 652                         /* the slave acknowledged and sent the requested data */
 653                         case 90:
 654                                 lss_buffer[0] = lss_get_value(msg->data+1);
 655                                 /* action ? */
 656                                 break;
 657                         case 91:
 658                                 lss_buffer[1] = lss_get_value(msg->data+1);
 659                                 /* action ? */
 660                                 break;
 661                         case 92:
 662                                 lss_buffer[2] = lss_get_value(msg->data+1);
 663                                 /* action ? */
 664                                 break;
 665                         case 93:
 666                                 lss_buffer[3] = lss_get_value(msg->data+1);
 667                                 /* action ? */
 668                                 break;
 669                 }
 670         }
 671
 672         else if (msg->cob_id.w == 0x07E5 /* 2021 */)
 673         {
 674                 // should be a slave
 675                 // the master sent a request
 676                 switch(msg->data[0])
 677                 {
 678                         case 4:
 679                                 lss_SwitchModeGlobal(d, msg->data[1]);
 680                                 break;
 681
 682                         case 17:
 683                                 lss_ConfigureNode_ID(d, msg->data[1]);
 684                                 break;
 685
 686                         case 19:
 687                                 lss_ConfigureBitTimingParameters(d, msg->data[1], msg->data[2]);
 688                                 break;
 689                         case 21:
 690                                 lss_ActivateBitTimingParameters_slave(msg->data[1], msg->data[2]);
 691                                 break;
 692
 693                         /* Switch Mode Selective */
 694                         case 64:
 695                                 lss_buffer[0] = lss_get_value(msg->data+1);
 696                                 break;
 697                         case 65:
 698                                 lss_buffer[1] = lss_get_value(msg->data+1);
 699                                 break;
 700                         case 66:
 701                                 lss_buffer[2] = lss_get_value(msg->data+1);
 702                                 break;
 703                         case 67:
 704                                 lss_buffer[3] = lss_get_value(msg->data+1);
 705                                 lss_SwitchModeSelective_slave(d);
 706                                 break;
 707
 708                         /* Identify Remote Slave */
 709                         case 70:
 710                                 lss_buffer[4] = lss_get_value(msg->data+1);
 711                                 break;
 712                         case 71:
 713                                 lss_buffer[5] = lss_get_value(msg->data+1);
 714                                 break;
 715                         case 72:
 716                                 lss_buffer[6] = lss_get_value(msg->data+1);
 717                                 break;
 718                         case 73:
 719                                 lss_buffer[7] = lss_get_value(msg->data+1);
 720                                 break;
 721                         case 74:
 722                                 lss_buffer[8] = lss_get_value(msg->data+1);
 723                                 break;
 724                         case 75:
 725                                 lss_buffer[9] = lss_get_value(msg->data+1);
 726                                 lss_IdentifySlave(d);
 727                                 break;
 728
 729                         /* Inquire Identify of Slave */
 730                         case 90:
 731                         case 91:
 732                         case 92:
 733                         case 93:
 734                                 lss_InquireLSSAddress_slave(d, msg->data[0]);
 735                                 break;
 736                 }
 737         }
 738
 739         return 0;
 740 }