esc.cpp

00001 /*
00002  * SOES Simple Open EtherCAT Slave
00003  *
00004  * File    : esc.c
00005  * Version : 1.0.0
00006  * Date    : 11-07-2010
00007  * Copyright (C) 2007-2010 Arthur Ketels
00008  *
00009  * SOES is free software; you can redistribute it and/or modify it under
00010  * the terms of the GNU General Public License version 2 as published by the Free
00011  * Software Foundation.
00012  *
00013  * SOES is distributed in the hope that it will be useful, but WITHOUT ANY
00014  * WARRANTY; without even the implied warranty of MERCHANTABILITY or
00015  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
00016  * for more details.
00017  *
00018  * As a special exception, if other files instantiate templates or use macros
00019  * or inline functions from this file, or you compile this file and link it
00020  * with other works to produce a work based on this file, this file does not
00021  * by itself cause the resulting work to be covered by the GNU General Public
00022  * License. However the source code for this file must still be made available
00023  * in accordance with section (3) of the GNU General Public License.
00024  *
00025  * This exception does not invalidate any other reasons why a work based on
00026  * this file might be covered by the GNU General Public License.
00027  *
00028  * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual
00029  * property of, and protected by Beckhoff Automation GmbH.
00030  */
00031 //#include <stdio.h>
00032 
00033 #ifdef TARGET_ATMEL
00034 #include <avr/io.h>
00035 #include <avr/pgmspace.h>
00036 #else
00037 #define PROGMEM
00038 #define pgm_read_dword(x) *(x)
00039 #define pgm_read_word(x) *(x)
00040 #define pgm_read_byte(x) *(x)
00041 
00042 #endif
00043 
00044 #include <string.h>
00045 #include "utypes.h"
00046 #include "esc.h"
00047 #include "objectlist.h"
00048 
00049 #define BITS2BYTES(b) ((b + 7) >> 3)
00050 
00051 uint16 SDO_findobject(uint16 index)
00052 {
00053   int16 n = 0;
00054   while (pgm_read_word(&SDOobjects[n].index) < index)
00055     {
00056       n++;
00057     }
00058   if (pgm_read_word(&SDOobjects[n].index) != index)
00059     {
00060       return -1;
00061     }
00062   return n;
00063 }
00064 
00065 uint16 sizeTXPDO(void)
00066 {
00067   uint8 c , l, si, sic;
00068   uint16 size = 0;
00069   int16 nidx;
00070   _objd FLASHSTORE *objd;
00071 
00072   if (pgm_read_word(&SDO1C13[0].data))
00073       si = *((uint8 *)pgm_read_word(&SDO1C13[0].data));
00074   else
00075       si = pgm_read_byte(&SDO1C13[0].value);
00076   if (si)
00077     {
00078       for (sic = 1 ; sic <= si ; sic++)
00079         {
00080           nidx = SDO_findobject(pgm_read_word(&SDO1C13[sic].value));
00081           if (nidx > 0)
00082             {
00083               objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
00084               l = pgm_read_byte(&objd->value);
00085               for (c = 1 ; c <= l ; c++)
00086                 size += (pgm_read_dword(&(objd + c)->value) & 0xff);
00087             }
00088         }
00089     }
00090   return BITS2BYTES(size);
00091 }
00092 
00093 uint16 sizeRXPDO(void)
00094 {
00095   uint8 c , l, si, sic;
00096   uint16 size = 0;
00097   int16 nidx;
00098   _objd FLASHSTORE *objd;
00099 
00100   if (pgm_read_word(&SDO1C12[0].data))
00101       si = *((uint8 *)pgm_read_word(&SDO1C12[0].data));
00102   else
00103       si = pgm_read_byte(&SDO1C12[0].value);
00104   if (si)
00105     {
00106       for (sic = 1 ; sic <= si ; sic++)
00107         {
00108           nidx = SDO_findobject(pgm_read_word(&SDO1C12[sic].value));
00109           if (nidx > 0)
00110             {
00111               objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
00112               l = pgm_read_byte(&objd->value);
00113               for (c = 1 ; c <= l ; c++)
00114                 size += (pgm_read_dword(&(objd + c)->value) & 0xff);
00115             }
00116         }
00117     }
00118   return BITS2BYTES(size);
00119 }
00120 
00121 void ESC_ALerror(uint16 errornumber)
00122 {
00123   uint16 dummy;
00124   ESCvar.ALerror = errornumber;
00125   dummy = htoes(errornumber);
00126   ESC_write(ESCREG_ALERROR, &dummy, sizeof(dummy), &ESCvar.ALevent);
00127 }
00128 
00129 void ESC_ALstatus(uint8 status)
00130 {
00131   uint16 dummy;
00132   ESCvar.ALstatus = status;
00133   dummy = htoes((uint16)status);
00134   ESC_write(ESCREG_ALSTATUS, &dummy, sizeof(dummy), &ESCvar.ALevent);
00135 }
00136 
00137 void ESC_SMack(uint8 n)
00138 {
00139   uint16 dummy;
00140   ESC_read(ESCREG_SM0STATUS + (n << 3), &dummy, 2, &ESCvar.ALevent);
00141 }
00142 
00143 void ESC_SMstatus(uint8 n)
00144 {
00145   _ESCsm2 *sm;
00146   uint16 temp;
00147   sm = (_ESCsm2 *)&ESCvar.SM[n];
00148   ESC_read(ESCREG_SM0STATUS + (n << 3), &temp, 2, &ESCvar.ALevent);
00149   temp = etohs(temp);
00150   sm->ActESC = temp >> 8;
00151   sm->Status = temp;
00152 }
00153 
00154 void ESC_SMwritepdi(uint8 n)
00155 {
00156   _ESCsm2 *sm;
00157   sm = (_ESCsm2 *)&ESCvar.SM[n];
00158   ESC_write(ESCREG_SM0PDI + (n << 3), &(sm->ActPDI), 1, &ESCvar.ALevent);
00159 }
00160 
00161 void ESC_SMenable(uint8 n)
00162 {
00163   _ESCsm2 *sm;
00164   sm = (_ESCsm2 *)&ESCvar.SM[n];
00165   sm->ActPDI &= ~ESCREG_SMENABLE_BIT;
00166   ESC_SMwritepdi(n);
00167 }
00168 
00169 void ESC_SMdisable(uint8 n)
00170 {
00171   _ESCsm2 *sm;
00172   sm = (_ESCsm2 *)&ESCvar.SM[n];
00173   sm->ActPDI |= ESCREG_SMENABLE_BIT;
00174   ESC_SMwritepdi(n);
00175 }
00176 
00177 void ESC_address(void)
00178 {
00179   ESC_read(ESCREG_ADDRESS, &ESCvar.address, sizeof(ESCvar.address), &ESCvar.ALevent);
00180   ESCvar.address = etohs(ESCvar.address);
00181 }
00182 
00183 uint8 ESC_WDstatus(void)
00184 {
00185   uint16 wdstatus;
00186   ESC_read(ESCREG_WDSTATUS, &wdstatus, 2, &ESCvar.ALevent);
00187   wdstatus = etohs(wdstatus);
00188   return (uint8)wdstatus;
00189 }
00190 
00191 uint8 ESC_checkmbx(uint8 state)
00192 {
00193   _ESCsm2 *SM;
00194   ESC_read(ESCREG_SM0, &ESCvar.SM[0], sizeof(ESCvar.SM[0]), &ESCvar.ALevent);
00195   ESC_read(ESCREG_SM1, &ESCvar.SM[1], sizeof(ESCvar.SM[1]), &ESCvar.ALevent);
00196   SM = (_ESCsm2 *)&ESCvar.SM[0];
00197   if ((etohs(SM->PSA) != MBX0_sma) || (etohs(SM->Length) != MBX0_sml) || (SM->Command != MBX0_smc) || (ESCvar.SM[0].ECsm == 0))
00198     {
00199       ESCvar.SMtestresult = SMRESULT_ERRSM0;
00200       ESC_SMdisable(0);
00201       ESC_SMdisable(1);
00202       return (uint8)(ESCinit | ESCerror); //fail state change
00203     }
00204   SM = (_ESCsm2 *)&ESCvar.SM[1];
00205   if ((etohs(SM->PSA) != MBX1_sma) || (etohs(SM->Length) != MBX1_sml) || (SM->Command != MBX1_smc) || (ESCvar.SM[1].ECsm == 0))
00206     {
00207       ESCvar.SMtestresult = SMRESULT_ERRSM1;
00208       ESC_SMdisable(0);
00209       ESC_SMdisable(1);
00210       return ESCinit | ESCerror; //fail state change
00211     }
00212   return state;
00213 }
00214 
00215 uint8 ESC_startmbx(uint8 state)
00216 {
00217   ESC_SMenable(0);
00218   ESC_SMenable(1);
00219   ESC_SMstatus(0);
00220   ESC_SMstatus(1);
00221   if ((state = ESC_checkmbx(state)) & ESCerror)
00222     {
00223       ESC_ALerror(ALERR_INVALIDMBXCONFIG);
00224       MBXrun = 0;
00225     }
00226   else
00227     {
00228       ESCvar.toggle = ESCvar.SM[1].ECrep; //sync repeat request toggle state
00229       MBXrun = 1;
00230     }
00231   return state;
00232 }
00233 
00234 void ESC_stopmbx(void)
00235 {
00236   uint8 n;
00237   MBXrun = 0;
00238   ESC_SMdisable(0);
00239   ESC_SMdisable(1);
00240   for (n = 0 ; n < MBXBUFFERS ; n++) MBXcontrol[n].state = MBXstate_idle;
00241   ESCvar.mbxoutpost = 0;
00242   ESCvar.mbxbackup = 0;
00243   ESCvar.xoe = 0;
00244   ESCvar.mbxfree = 1;
00245   ESCvar.toggle = 0;
00246   ESCvar.mbxincnt = 0;
00247   ESCvar.segmented = 0;
00248   ESCvar.frags = 0;
00249   ESCvar.fragsleft = 0;
00250   ESCvar.txcue = 0;
00251 }
00252 
00253 void ESC_readmbx(void)
00254 {
00255   _MBX *MB = &MBX[0];
00256   uint16 length;
00257   ESC_read(MBX0_sma, MB,MBXHSIZE, &ESCvar.ALevent);
00258   length = etohs(MB->header.length);
00259   if (length > (MBX0_sml - MBXHSIZE)) length = MBX0_sml - MBXHSIZE;
00260   ESC_read(MBX0_sma + MBXHSIZE, &(MB->b[0]), length, &ESCvar.ALevent);
00261   if (length + MBXHSIZE < MBX0_sml)
00262     {
00263       ESC_read(MBX0_sme, &length, 1, &ESCvar.ALevent);
00264     }
00265   MBXcontrol[0].state = MBXstate_inclaim;
00266 }
00267 
00268 void ESC_writembx(uint8 n)
00269 {
00270   _MBX *MB = &MBX[n];
00271   uint8 dummy = 0;
00272   uint16 length;
00273   length = etohs(MB->header.length);
00274   if (length > (MBX1_sml - MBXHSIZE)) length = MBX1_sml - MBXHSIZE;
00275   ESC_write(MBX1_sma, MB, MBXHSIZE + length, &ESCvar.ALevent);
00276   if (length + MBXHSIZE < MBX1_sml)
00277     {
00278       ESC_write(MBX1_sme, &dummy, 1, &ESCvar.ALevent);
00279     }
00280   ESCvar.mbxfree = 0;
00281 }
00282 
00283 void ESC_ackmbxread(void)
00284 {
00285   uint8 dummy = 0;
00286   ESC_write(MBX1_sma, &dummy, 1, &ESCvar.ALevent);
00287   ESCvar.mbxfree = 1;
00288 }
00289 
00290 uint8 ESC_claimbuffer(void)
00291 {
00292   _MBX *MB;
00293   uint8 n = MBXBUFFERS - 1;
00294   while ((n > 0) && (MBXcontrol[n].state)) n--;
00295   if (n)
00296     {
00297       MBXcontrol[n].state = MBXstate_outclaim;
00298       MB = &MBX[n];
00299       ESCvar.mbxcnt = (++ESCvar.mbxcnt) & 0x07;
00300       if (ESCvar.mbxcnt == 0) ESCvar.mbxcnt = 1;
00301       MB->header.address = htoes(0x0000); // destination is master
00302       MB->header.channel = 0;
00303       MB->header.priority = 0;
00304       MB->header.mbxcnt = ESCvar.mbxcnt;
00305       ESCvar.txcue++;
00306     }
00307   return n;
00308 }
00309 
00310 uint8 ESC_outreqbuffer(void)
00311 {
00312   uint8 n = MBXBUFFERS-1;
00313   while ((n > 0) && (MBXcontrol[n].state != MBXstate_outreq)) n--;
00314   return n;
00315 }
00316 
00317 void MBX_error(uint16 error)
00318 {
00319   uint8 MBXout;
00320   _MBXerr *mbxerr;
00321   MBXout = ESC_claimbuffer();
00322   if (MBXout)
00323     {
00324       mbxerr = (_MBXerr *)&MBX[MBXout];
00325       mbxerr->mbxheader.length = htoes((uint16)0x04);
00326       mbxerr->mbxheader.mbxtype = MBXERR;
00327       mbxerr->type = htoes((uint16)0x01);
00328       mbxerr->detail = htoes(error);
00329       MBXcontrol[MBXout].state = MBXstate_outreq;
00330     }
00331 }
00332 
00333 uint8 ESC_mbxprocess(void)
00334 {
00335   uint8 mbxhandle = 0;
00336   _MBX *MB = &MBX[0];
00337 
00338   if (!MBXrun)
00339     {
00340       return 0;  //nothing to do
00341     }
00342 
00343   if (ESCvar.ALevent & ESCREG_ALEVENT_SM_MASK) // SM0/1 access or SMn change event
00344     {
00345       ESC_SMstatus(0);
00346       ESC_SMstatus(1);
00347     }
00348 
00349   if (ESCvar.mbxoutpost && ESCvar.SM[1].IntR) //outmbx read by master
00350     {
00351       ESC_ackmbxread();
00352       if (ESCvar.mbxbackup) // dispose old backup
00353         {
00354           MBXcontrol[ESCvar.mbxbackup].state = MBXstate_idle;
00355         }
00356       if (MBXcontrol[ESCvar.mbxoutpost].state == MBXstate_again) // if still to do
00357         {
00358           ESC_writembx(ESCvar.mbxoutpost);
00359         }
00360       MBXcontrol[ESCvar.mbxoutpost].state = MBXstate_backup; //create new backup
00361       ESCvar.mbxbackup = ESCvar.mbxoutpost;
00362       ESCvar.mbxoutpost = 0;
00363       return 0;
00364     }
00365 
00366   if (ESCvar.SM[1].ECrep != ESCvar.toggle) // repeat request
00367     {
00368       if (ESCvar.mbxoutpost || ESCvar.mbxbackup)
00369         {
00370           if (!ESCvar.mbxoutpost) // if outmbx empty
00371             {
00372               ESC_writembx(ESCvar.mbxbackup); // use backup mbx
00373             }
00374           else
00375             {
00376               ESC_SMdisable(1); // reset mailbox
00377               MBXcontrol[ESCvar.mbxoutpost].state = MBXstate_again; // have to resend later
00378               ESC_SMenable(1); // activate mailbox
00379               ESC_writembx(ESCvar.mbxbackup); // use backup mbx
00380             }
00381           ESCvar.toggle = ESCvar.SM[1].ECrep;
00382           ESCvar.SM[1].PDIrep = ESCvar.toggle;
00383           ESC_SMwritepdi(1);
00384         }
00385       return 0;
00386     }
00387 
00388 // if the outmailbox is free check if we have something to send
00389   if (ESCvar.txcue && (ESCvar.mbxfree || !ESCvar.SM[1].MBXstat))
00390     {
00391       mbxhandle = ESC_outreqbuffer(); // check out request mbx
00392       if (mbxhandle) // outmbx empty and outreq mbx available
00393         {
00394           ESC_writembx(mbxhandle);
00395           MBXcontrol[mbxhandle].state = MBXstate_outpost; // change state
00396           ESCvar.mbxoutpost = mbxhandle;
00397           if (ESCvar.txcue) ESCvar.txcue--;
00398         }
00399     }
00400 
00401 // read mailbox if full and no xoe in progress
00402   if (ESCvar.SM[0].MBXstat && !MBXcontrol[0].state && !ESCvar.mbxoutpost && !ESCvar.xoe)
00403     {
00404       ESC_readmbx();
00405       ESCvar.SM[0].MBXstat = 0;
00406       if (etohs(MB->header.length) < 8)
00407         {
00408           if (etohs(MB->header.length) == 0)
00409             MBX_error(MBXERR_INVALIDHEADER);
00410           else
00411             MBX_error(MBXERR_SIZETOOSHORT);
00412           MBXcontrol[0].state = MBXstate_idle; // drop mailbox
00413         }
00414       if ((MB->header.mbxcnt !=0) && (MB->header.mbxcnt == ESCvar.mbxincnt))
00415         {
00416           MBXcontrol[0].state = MBXstate_idle; // drop mailbox
00417         }
00418       ESCvar.mbxincnt = MB->header.mbxcnt;
00419       return 1;
00420     }
00421 
00422   if (ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE) // ack changes in non used SM
00423     {
00424       ESC_SMack(4);
00425       ESC_SMack(5);
00426       ESC_SMack(6);
00427       ESC_SMack(7);
00428     }
00429 
00430   return 0;
00431 }
00432 
00433 int16 SDO_findsubindex(int16 nidx, uint8 subindex)
00434 {
00435   _objd FLASHSTORE *objd;
00436   int16 n = 0;
00437   uint8 maxsub;
00438   objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
00439   maxsub = pgm_read_byte(&SDOobjects[nidx].maxsub);
00440   while (( pgm_read_byte(&(objd + n)->subindex) < subindex) && (n < maxsub))
00441     {
00442       n++;
00443     }
00444   if (pgm_read_byte(&(objd + n)->subindex) != subindex)
00445     {
00446       return -1;
00447     }
00448   return n;
00449 }
00450 
00451 void copy2mbx(void *source, void *dest, uint16 size)
00452 {
00453   memcpy(dest, source, size);
00454 }
00455 
00456 void SDO_abort(uint16 index, uint8 subindex, uint32 abortcode)
00457 {
00458   uint8 MBXout;
00459   _COEsdo *coeres;
00460   MBXout = ESC_claimbuffer();
00461   if (MBXout)
00462     {
00463       coeres = (_COEsdo *)&MBX[MBXout];
00464       coeres->mbxheader.length = htoes(COE_DEFAULTLENGTH);
00465       coeres->mbxheader.mbxtype = MBXCOE;
00466       coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOREQUEST << 12));
00467       coeres->index = htoes(index);
00468       coeres->subindex = subindex;
00469       coeres->command = COE_COMMAND_SDOABORT;
00470       coeres->size = htoel(abortcode);
00471       MBXcontrol[MBXout].state = MBXstate_outreq;
00472     }
00473 }
00474 
00475 void SDO_upload(void)
00476 {
00477   _COEsdo *coesdo,*coeres;
00478   uint16 index;
00479   uint8 subindex;
00480   int16 nidx, nsub;
00481   uint8 MBXout;
00482   uint16 size;
00483   uint8 dss;
00484   _objd FLASHSTORE *objd;
00485   coesdo = (_COEsdo *)&MBX[0];
00486   index = etohs(coesdo->index);
00487   subindex = coesdo->subindex;
00488   nidx = SDO_findobject(index);
00489   if (nidx >= 0)
00490     {
00491       nsub = SDO_findsubindex(nidx, subindex);
00492       if (nsub >= 0)
00493         {
00494           objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
00495           MBXout = ESC_claimbuffer();
00496           if (MBXout)
00497             {
00498               coeres = (_COEsdo *)&MBX[MBXout];
00499               coeres->mbxheader.length = htoes(COE_DEFAULTLENGTH);
00500               coeres->mbxheader.mbxtype = MBXCOE;
00501               coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDORESPONSE << 12));
00502               size = pgm_read_word(&(objd + nsub)->bitlength);
00503               // expedited bits used calculation
00504               dss = 0x0c;
00505               if (size>8) dss = 0x08;
00506               if (size>16) dss = 0x04;
00507               if (size>24) dss = 0x00;
00508               coeres->index = htoes(index);
00509               coeres->subindex = subindex;
00510               if (size <= 32)
00511                 { // expedited response i.e. length<=4 bytes
00512                   coeres->command = COE_COMMAND_UPLOADRESPONSE +
00513                                     COE_SIZE_INDICATOR +
00514                                     COE_EXPEDITED_INDICATOR + dss;
00515                   if (pgm_read_word(&(objd+nsub)->data) == nil)
00516                     {
00517                       coeres->size = htoel(pgm_read_dword(&(objd + nsub)->value)); //use constant value
00518                     }
00519                   else
00520                     {
00521                       size = (size + 7) >> 3; //convert bits to bytes
00522                       copy2mbx((void *)pgm_read_word(&(objd + nsub)->data), &(coeres->size), size); //use dynamic data
00523                     }
00524                 }
00525               else
00526                 { // normal response i.e. length>4 bytes
00527                   coeres->command = COE_COMMAND_UPLOADRESPONSE +
00528                                     COE_SIZE_INDICATOR;
00529                   size=(size + 7) >> 3; //convert bits to bytes
00530                   coeres->size = htoel(size);
00531                   if ((size + COE_HEADERSIZE) > MBXDSIZE)
00532                     { // segemented transfer needed
00533                       ESCvar.frags = size; // set total size in bytes
00534                       size = MBXDSIZE - COE_HEADERSIZE; //limit to mailbox size
00535                       ESCvar.fragsleft = size; // number of bytes done
00536                       ESCvar.segmented = MBXSEU; // signal segmented transfer
00537                       ESCvar.data = (void *)pgm_read_word(&(objd + nsub)->data);
00538                     }
00539                   else
00540                     {
00541                       ESCvar.segmented = 0;
00542                     }
00543                   coeres->mbxheader.length = htoes(COE_HEADERSIZE + size);
00544                   copy2mbx((void *)pgm_read_word(&(objd + nsub)->data), (&(coeres->size)) + 1, size); //use dynamic data
00545                 }
00546               MBXcontrol[MBXout].state = MBXstate_outreq;
00547             }
00548         }
00549       else
00550         {
00551           SDO_abort(index, subindex, ABORT_NOSUBINDEX);
00552         }
00553     }
00554   else
00555     {
00556       SDO_abort(index, subindex, ABORT_NOOBJECT);
00557     }
00558   MBXcontrol[0].state = MBXstate_idle;
00559   ESCvar.xoe = 0;
00560 }
00561 
00562 
00563 void SDO_uploadsegment(void)
00564 {
00565   _COEsdo *coesdo, *coeres;
00566   uint8 MBXout;
00567   uint16 size, offset;
00568   coesdo = (_COEsdo *)&MBX[0];
00569   MBXout = ESC_claimbuffer();
00570   if (MBXout)
00571     {
00572       coeres = (_COEsdo *)&MBX[MBXout];
00573       offset = ESCvar.fragsleft;
00574       size = ESCvar.frags - ESCvar.fragsleft;
00575       coeres->mbxheader.mbxtype = MBXCOE;
00576       coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDORESPONSE << 12));
00577       coeres->command = COE_COMMAND_UPLOADSEGMENT +
00578                         (coesdo->command & COE_TOGGLEBIT); // copy toggle bit
00579       if ((size + COE_SEGMENTHEADERSIZE) > MBXDSIZE)
00580         { // more segemented transfer needed
00581           size = MBXDSIZE - COE_SEGMENTHEADERSIZE; //limit to mailbox size
00582           ESCvar.fragsleft += size; // number of bytes done
00583           coeres->mbxheader.length = htoes(COE_SEGMENTHEADERSIZE + size);
00584         }
00585       else
00586         {// last segment
00587           ESCvar.segmented = 0;
00588           ESCvar.frags = 0;
00589           ESCvar.fragsleft = 0;
00590           coeres->command += COE_COMMAND_LASTSEGMENTBIT;
00591           if (size >= 7)
00592             {
00593               coeres->mbxheader.length = htoes(COE_SEGMENTHEADERSIZE + size);
00594             }
00595           else
00596             {
00597               coeres->command += (7 - size) << 1;
00598               coeres->mbxheader.length = htoes(COE_DEFAULTLENGTH);
00599             }
00600         }
00601       copy2mbx((uint8*)ESCvar.data + offset, (&(coeres->command)) + 1, size); //copy to mailbox
00602 
00603       MBXcontrol[MBXout].state = MBXstate_outreq;
00604     }
00605   MBXcontrol[0].state = MBXstate_idle;
00606   ESCvar.xoe = 0;
00607 }
00608 
00609 void SDO_download(void)
00610 {
00611   _COEsdo *coesdo, *coeres;
00612   uint16 index;
00613   uint8 subindex;
00614   int16 nidx, nsub;
00615   uint8 MBXout;
00616   uint16 size, actsize;
00617   _objd FLASHSTORE *objd;
00618   uint32 *mbxdata;
00619   coesdo = (_COEsdo *)&MBX[0];
00620   index = etohs(coesdo->index);
00621   subindex = coesdo->subindex;
00622   nidx = SDO_findobject(index);
00623   if (nidx >= 0)
00624     {
00625       nsub = SDO_findsubindex(nidx, subindex);
00626       if (nsub >= 0)
00627         {
00628           objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
00629           if ((pgm_read_word(&(objd + nsub)->access) == ATYPE_RW) ||
00630               ((pgm_read_word(&(objd + nsub)->access) == ATYPE_RWpre) && ((ESCvar.ALstatus & 0x0f) == ESCpreop)))
00631             {
00632               if (coesdo->command & COE_EXPEDITED_INDICATOR) //expedited?
00633                 {
00634                   size = 4 - ((coesdo->command & 0x0c) >> 2);
00635                   mbxdata = &(coesdo->size);
00636                 }
00637               else // normal upload
00638                 {
00639                   size = (etohs(coesdo->size) & 0xffff);
00640                   mbxdata = (&(coesdo->size)) + 1;
00641                 }
00642               actsize = (pgm_read_word(&(objd + nsub)->bitlength) + 7) >> 3;
00643               if ( actsize == size )
00644                 {
00645                   copy2mbx(mbxdata, (void *)pgm_read_word(&(objd + nsub)->data), size);
00646                   MBXout = ESC_claimbuffer();
00647                   if (MBXout)
00648                     {
00649                       coeres = (_COEsdo *)&MBX[MBXout];
00650                       coeres->mbxheader.length = htoes(COE_DEFAULTLENGTH);
00651                       coeres->mbxheader.mbxtype = MBXCOE;
00652                       coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDORESPONSE << 12));
00653                       coeres->index = htoes(index);
00654                       coeres->subindex = subindex;
00655                       coeres->command = COE_COMMAND_DOWNLOADRESPONSE;
00656                       coeres->size = htoel(0);
00657                       MBXcontrol[MBXout].state = MBXstate_outreq;
00658                     }
00659                   // external object write handler
00660                   ESC_objecthandler(index, subindex);
00661                 }
00662               else
00663                 {
00664                   SDO_abort(index, subindex, ABORT_TYPEMISMATCH);
00665                 }
00666             }
00667           else
00668             {
00669               if(pgm_read_word(&(objd + nsub)->access) == ATYPE_RWpre)
00670                   SDO_abort(index, subindex, ABORT_NOTINTHISSTATE);
00671               else
00672                   SDO_abort(index, subindex, ABORT_READONLY);
00673             }
00674         }
00675       else
00676         {
00677           SDO_abort(index, subindex, ABORT_NOSUBINDEX);
00678         }
00679     }
00680   else
00681     {
00682       SDO_abort(index, subindex, ABORT_NOOBJECT);
00683     }
00684   MBXcontrol[0].state = MBXstate_idle;
00685   ESCvar.xoe = 0;
00686 }
00687 
00688 void SDO_infoerror(uint32 abortcode)
00689 {
00690   uint8 MBXout;
00691   _COEobjdesc *coeres;
00692   MBXout = ESC_claimbuffer();
00693   if (MBXout)
00694     {
00695       coeres = (_COEobjdesc *)&MBX[MBXout];
00696       coeres->mbxheader.length = htoes((uint16)0x0a);
00697       coeres->mbxheader.mbxtype = MBXCOE;
00698       coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
00699       coeres->infoheader.opcode = COE_INFOERROR; //SDO info error request
00700       coeres->infoheader.incomplete = 0;
00701       coeres->infoheader.reserved = 0x00;
00702       coeres->infoheader.fragmentsleft = 0;
00703       coeres->index = htoel(abortcode);
00704       MBXcontrol[MBXout].state = MBXstate_outreq;
00705     }
00706 }
00707 
00708 #define ODLISTSIZE  ((MBX1_sml - MBXHSIZE - sizeof(_COEh) - sizeof(_INFOh) - 2) & 0xfffe)
00709 
00710 void SDO_getodlist(void)
00711 {
00712   uint16 frags;
00713   uint8 MBXout = 0;
00714   uint16 entries = 0;
00715   uint16 i, n;
00716   uint16 *p;
00717   _COEobjdesc *coel, *coer;
00718 
00719   while (pgm_read_word(&SDOobjects[entries].index) != 0xffff) entries++;
00720   ESCvar.entries = entries;
00721   frags = ((entries << 1) + ODLISTSIZE - 1);
00722   frags /= ODLISTSIZE;
00723   coer = (_COEobjdesc *)&MBX[0];
00724   if (etohs(coer->index) > 0x01) // check for unsupported opcodes
00725     {
00726       SDO_infoerror(ABORT_UNSUPPORTED);
00727     }
00728   else
00729     {
00730       MBXout = ESC_claimbuffer();
00731     }
00732   if (MBXout)
00733     {
00734       coel = (_COEobjdesc *)&MBX[MBXout];
00735       coel->mbxheader.mbxtype = MBXCOE;
00736       coel->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
00737       coel->infoheader.opcode = COE_GETODLISTRESPONSE;
00738       if (etohs(coer->index) == 0x00) //number of objects request
00739         {
00740           coel->index = htoes((uint16)0x00);
00741           coel->infoheader.incomplete = 0;
00742           coel->infoheader.reserved = 0x00;
00743           coel->infoheader.fragmentsleft = htoes((uint16)0);
00744           MBXcontrol[0].state = MBXstate_idle;
00745           ESCvar.xoe = 0;
00746           ESCvar.frags = frags;
00747           ESCvar.fragsleft = frags - 1;
00748           p = &(coel->datatype);
00749           *p = htoes(entries);
00750           p++;
00751           *p = 0;
00752           p++;
00753           *p = 0;
00754           p++;
00755           *p = 0;
00756           p++;
00757           *p = 0;
00758           coel->mbxheader.length = htoes(0x08 + (5 << 1));
00759         }
00760       if (etohs(coer->index) == 0x01) //only return all objects
00761         {
00762           if (frags > 1)
00763             {
00764               coel->infoheader.incomplete = 1;
00765               ESCvar.xoe = MBXCOE + MBXODL;
00766               n = ODLISTSIZE >> 1;
00767             }
00768           else
00769             {
00770               coel->infoheader.incomplete = 0;
00771               MBXcontrol[0].state = MBXstate_idle;
00772               ESCvar.xoe = 0;
00773               n = entries;
00774             }
00775           coel->infoheader.reserved = 0x00;
00776           ESCvar.frags = frags;
00777           ESCvar.fragsleft = frags - 1;
00778           coel->infoheader.fragmentsleft = htoes(ESCvar.fragsleft);
00779           coel->index = htoes((uint16)0x01);
00780 
00781           p = &(coel->datatype);
00782           for (i = 0 ; i < n ; i++)
00783             {
00784               *p = htoes(pgm_read_word(&SDOobjects[i].index));
00785               p++;
00786             }
00787 
00788           coel->mbxheader.length = htoes(0x08 + ( n << 1));
00789         }
00790       MBXcontrol[MBXout].state = MBXstate_outreq;
00791     }
00792 }
00793 
00794 void SDO_getodlistcont(void)
00795 {
00796   uint8 MBXout;
00797   uint16 i , n, s;
00798   uint16 *p;
00799   _COEobjdesc *coel;
00800 
00801   MBXout = ESC_claimbuffer();
00802   if (MBXout)
00803     {
00804       coel = (_COEobjdesc *)&MBX[MBXout];
00805       coel->mbxheader.mbxtype = MBXCOE;
00806       coel->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
00807       coel->infoheader.opcode = COE_GETODLISTRESPONSE;
00808       s = (ESCvar.frags - ESCvar.fragsleft) * (ODLISTSIZE >> 1);
00809       if (ESCvar.fragsleft > 1)
00810         {
00811           coel->infoheader.incomplete = 1;
00812           n = s + (ODLISTSIZE >> 1);
00813         }
00814       else
00815         {
00816           coel->infoheader.incomplete = 0;
00817           MBXcontrol[0].state = MBXstate_idle;
00818           ESCvar.xoe = 0;
00819           n = ESCvar.entries;
00820         }
00821       coel->infoheader.reserved = 0x00;
00822       ESCvar.fragsleft--;
00823       coel->infoheader.fragmentsleft = htoes(ESCvar.fragsleft);
00824       p = &(coel->index); //pointer 2 bytes back to exclude index
00825       for ( i = s ; i < n ; i++)
00826         {
00827           *p = htoes(pgm_read_word(&SDOobjects[i].index));
00828           p++;
00829         }
00830       coel->mbxheader.length = htoes(0x06 + ((n - s) << 1));
00831       MBXcontrol[MBXout].state = MBXstate_outreq;
00832     }
00833 }
00834 
00835 void SDO_getod(void)
00836 {
00837   uint8 MBXout;
00838   uint16 index;
00839   int16 nidx;
00840   uint8 *d;
00841   uint8 FLASHSTORE *s;
00842   uint8 n = 0;
00843   _COEobjdesc *coer, *coel;
00844   coer = (_COEobjdesc *)&MBX[0];
00845   index = etohs(coer->index);
00846   nidx = SDO_findobject(index);
00847   if (nidx >= 0)
00848     {
00849       MBXout = ESC_claimbuffer();
00850       if (MBXout)
00851         {
00852           coel = (_COEobjdesc *)&MBX[MBXout];
00853           coel->mbxheader.mbxtype = MBXCOE;
00854           coel->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
00855           coel->infoheader.opcode = COE_GETODRESPONSE;
00856           coel->infoheader.incomplete = 0;
00857           coel->infoheader.reserved = 0x00;
00858           coel->infoheader.fragmentsleft = htoes(0);
00859           coel->index = htoes(index);
00860           coel->datatype = htoes(0);
00861           coel->maxsub = pgm_read_byte(&SDOobjects[nidx].maxsub);
00862           coel->objectcode = pgm_read_word(&SDOobjects[nidx].objtype);
00863           s = (uint8 *)pgm_read_word(&SDOobjects[nidx].name);
00864           d = (uint8 *)&(coel->name);
00865           while (pgm_read_byte(s) && (n < (MBXDSIZE - 0x0c)))
00866             {
00867               *d = pgm_read_byte(s);
00868               n++;
00869               s++;
00870               d++;
00871             }
00872           *d = pgm_read_byte(s);
00873           coel->mbxheader.length = htoes((uint16)0x0c + n);
00874           MBXcontrol[MBXout].state = MBXstate_outreq;
00875           MBXcontrol[0].state = MBXstate_idle;
00876           ESCvar.xoe=0;
00877         }
00878     }
00879   else
00880     {
00881       SDO_infoerror(ABORT_NOOBJECT);
00882     }
00883 }
00884 
00885 void SDO_geted(void)
00886 {
00887   uint8 MBXout;
00888   uint16 index;
00889   int16 nidx, nsub;
00890   uint8 subindex;
00891   uint8 *d;
00892   uint8 FLASHSTORE *s;
00893   _objd FLASHSTORE *objd;
00894   uint8 n = 0;
00895   _COEentdesc *coer, *coel;
00896   coer = (_COEentdesc *)&MBX[0];
00897   index = etohs(coer->index);
00898   subindex = coer->subindex;
00899   nidx = SDO_findobject(index);
00900   if (nidx >= 0)
00901     {
00902       nsub = SDO_findsubindex(nidx, subindex);
00903       if (nsub >= 0)
00904         {
00905           objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
00906           MBXout = ESC_claimbuffer();
00907           if (MBXout)
00908             {
00909               coel = (_COEentdesc *)&MBX[MBXout];
00910               coel->mbxheader.mbxtype = MBXCOE;
00911               coel->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
00912               coel->infoheader.opcode = COE_ENTRYDESCRIPTIONRESPONSE;
00913               coel->infoheader.incomplete = 0;
00914               coel->infoheader.reserved = 0x00;
00915               coel->infoheader.fragmentsleft = htoes((uint16)0);
00916               coel->index = htoes(index);
00917               coel->subindex = subindex;
00918               coel->valueinfo = COE_VALUEINFO_ACCESS +
00919                                 COE_VALUEINFO_OBJECT +
00920                                 COE_VALUEINFO_MAPPABLE;
00921               coel->datatype = htoes(pgm_read_word(&(objd + nsub)->datatype));
00922               coel->bitlength = htoes(pgm_read_word(&(objd + nsub)->bitlength));
00923               coel->access = htoes(pgm_read_word(&(objd + nsub)->access));
00924               s = (uint8 *)pgm_read_word(&(objd+nsub)->name);
00925               d = (uint8 *)&(coel->name);
00926               while (pgm_read_byte(s) && (n < (MBXDSIZE - 0x10)))
00927                 {
00928                   *d = pgm_read_byte(s);
00929                   n++;
00930                   s++;
00931                   d++;
00932                 }
00933               *d = pgm_read_byte(s);
00934               coel->mbxheader.length = htoes((uint16)0x10 + n);
00935               MBXcontrol[MBXout].state = MBXstate_outreq;
00936               MBXcontrol[0].state = MBXstate_idle;
00937               ESCvar.xoe = 0;
00938             }
00939         }
00940       else
00941         {
00942           SDO_infoerror(ABORT_NOSUBINDEX);
00943         }
00944     }
00945   else
00946     {
00947       SDO_infoerror(ABORT_NOOBJECT);
00948     }
00949 }
00950 
00951 void ESC_coeprocess(void)
00952 {
00953   _MBXh *mbh;
00954   _COEsdo *coesdo;
00955   _COEobjdesc *coeobjdesc;
00956   uint8 service;
00957   if (!MBXrun)
00958     {
00959       return;
00960     }
00961   if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
00962     {
00963       mbh = (_MBXh *)&MBX[0];
00964       if (mbh->mbxtype == MBXCOE)
00965         {
00966           ESCvar.xoe = MBXCOE;
00967         }
00968     }
00969   if ((ESCvar.xoe == (MBXCOE + MBXODL)) && (!ESCvar.mbxoutpost))
00970     {
00971       SDO_getodlistcont(); // continue get OD list
00972     }
00973   if (ESCvar.xoe == MBXCOE)
00974     {
00975       coesdo = (_COEsdo *)&MBX[0];
00976       coeobjdesc = (_COEobjdesc *)&MBX[0];
00977       service = etohs(coesdo->coeheader.numberservice) >> 12;
00978       //initiate SDO upload request
00979       if ((service == COE_SDOREQUEST)
00980           && (coesdo->command == COE_COMMAND_UPLOADREQUEST)
00981           && (etohs(coesdo->mbxheader.length) == 0x0a))
00982         {
00983           SDO_upload();
00984         }
00985       //SDO upload segment request
00986       if ((service == COE_SDOREQUEST)
00987           && ((coesdo->command & 0xef) == COE_COMMAND_UPLOADSEGREQ)
00988           && (etohs(coesdo->mbxheader.length) == 0x0a)
00989           && (ESCvar.segmented == MBXSEU))
00990         {
00991           SDO_uploadsegment();
00992         }
00993       //initiate SDO download request
00994       else
00995         if ((service == COE_SDOREQUEST)
00996             && ((coesdo->command & 0xf1) == 0x21))
00997           {
00998             SDO_download();
00999           }
01000       //initiate SDO get OD list
01001         else
01002           if ((service == COE_SDOINFORMATION)
01003               &&(coeobjdesc->infoheader.opcode == 0x01))
01004             {
01005               SDO_getodlist();
01006             }
01007       //initiate SDO get OD
01008           else
01009             if ((service == COE_SDOINFORMATION)
01010                 && (coeobjdesc->infoheader.opcode == 0x03))
01011               {
01012                 SDO_getod();
01013               }
01014       //initiate SDO get ED
01015             else
01016               if ((service == COE_SDOINFORMATION)
01017                   && (coeobjdesc->infoheader.opcode == 0x05))
01018                 {
01019                   SDO_geted();
01020                 }
01021               else
01022                 if (ESCvar.xoe == MBXCOE) //COE not recognised above
01023                   {
01024                     if (service == 0)
01025                     {
01026                       MBX_error(MBXERR_INVALIDHEADER);
01027                     }
01028                     else
01029                     {
01030                       SDO_abort(etohs(coesdo->index), coesdo->subindex, ABORT_UNKNOWN);
01031                     }
01032                     MBXcontrol[0].state = MBXstate_idle;
01033                     ESCvar.xoe = 0;
01034                   }
01035     }
01036 }
01037 
01038 void ESC_xoeprocess(void)
01039 {
01040   _MBXh *mbh;
01041   if (!MBXrun)
01042     {
01043       return;
01044     }
01045   if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
01046     {
01047       mbh = (_MBXh *)&MBX[0];
01048       if ((mbh->mbxtype == 0) || (etohs(mbh->length) == 0))
01049       {
01050         MBX_error(MBXERR_INVALIDHEADER);
01051       }
01052       else
01053       {
01054         MBX_error(MBXERR_UNSUPPORTEDPROTOCOL);
01055       }
01056       MBXcontrol[0].state = MBXstate_idle; // mailbox type not supported, drop mailbox
01057     }
01058 }
01059 
01060 uint8 ESC_checkSM23(uint8 state)
01061 {
01062   _ESCsm2 *SM;
01063   ESC_read(ESCREG_SM2, &ESCvar.SM[2], sizeof(ESCvar.SM[2]), &ESCvar.ALevent);
01064   ESC_read(ESCREG_SM3, &ESCvar.SM[3], sizeof(ESCvar.SM[3]), &ESCvar.ALevent);
01065   SM = (_ESCsm2 *)&ESCvar.SM[2];
01066   if ((etohs(SM->PSA) != SM2_sma) || (etohs(SM->Length) != SM2_sml) || (SM->Command != SM2_smc)  || !(SM->ActESC & SM2_act))
01067     {
01068       ESCvar.SMtestresult = SMRESULT_ERRSM2;
01069       return ESCpreop | ESCerror; //fail state change
01070     }
01071   SM = (_ESCsm2 *)&ESCvar.SM[3];
01072   if ((etohs(SM->PSA) != SM3_sma) || (etohs(SM->Length) != SM3_sml) || (SM->Command != SM3_smc) || !(SM->ActESC & SM3_act))
01073     {
01074       ESCvar.SMtestresult = SMRESULT_ERRSM3;
01075       return ESCpreop | ESCerror; //fail state change
01076     }
01077   return state;
01078 }
01079 
01080 uint8 ESC_startinput(uint8 state)
01081 {
01082   state = ESC_checkSM23(state);
01083   if (state != (ESCpreop | ESCerror))
01084     {
01085       ESC_SMenable(3);
01086       APPstate = APPSTATE_INPUT;
01087     }
01088   else
01089     {
01090       ESC_SMdisable(2);
01091       ESC_SMdisable(3);
01092       if( ESCvar.SMtestresult & SMRESULT_ERRSM3 )
01093         ESC_ALerror(ALERR_INVALIDINPUTSM);
01094       else
01095         ESC_ALerror(ALERR_INVALIDOUTPUTSM);
01096     }
01097   return state;
01098 }
01099 
01100 void ESC_stopinput(void)
01101 {
01102   APPstate = APPSTATE_IDLE;
01103   ESC_SMdisable(3);
01104   ESC_SMdisable(2);
01105 }
01106 
01107 uint8 ESC_startoutput(uint8 state)
01108 {
01109   ESC_SMenable(2);
01110   APPstate |= APPSTATE_OUTPUT;
01111   return state;
01112 }
01113 
01114 void ESC_stopoutput(void)
01115 {
01116   APPstate &= APPSTATE_INPUT;
01117   ESC_SMdisable(2);
01118   APP_safeoutput();
01119 }
01120 
01121 void ESC_ALevent(void)
01122 {
01123   if (!(ESCvar.ALevent & (ESCREG_ALEVENT_CONTROL | ESCREG_ALEVENT_SMCHANGE)) &&
01124       (ESCvar.wdcnt++ < 1000))
01125     { //nothing to do
01126       ESCvar.ALcontrol = 0;
01127       return;
01128     }
01129   ESCvar.wdcnt = 0;
01130   ESC_read(ESCREG_ALCONTROL, &ESCvar.ALcontrol, sizeof(ESCvar.ALcontrol), &ESCvar.ALevent);
01131   ESCvar.ALcontrol = etohs(ESCvar.ALcontrol);
01132 }
01133 
01134 void ESC_state(void)
01135 {
01136   uint8 ac, an, as, ax;
01137   if (!ESCvar.ALcontrol) return; //nothing to do
01138   ac = ESCvar.ALcontrol & 0x001f;
01139   as = ESCvar.ALstatus & 0x001f;
01140   an = as;
01141   if (((ac & ESCerror) || (ac == ESCinit)))
01142     {
01143       ac &= 0x0f;  // if error bit confirmed reset
01144       an &= 0x0f;
01145     }
01146   if ((ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE) &&
01147       (as & 0x0e) &&
01148       !(ESCvar.ALevent & ESCREG_ALEVENT_CONTROL) &&
01149       MBXrun)
01150     {
01151       ESCvar.ALevent = 0;
01152       ax = ESC_checkmbx(as);
01153       if ((as & ESCerror) && ((ac != (ESCinit | ESCerror)) || (ac != ESCinit)))
01154         return; // if in error then stay there
01155       if (ax == (ESCinit | ESCerror))
01156         {
01157           if (APPstate)
01158             {
01159               ESC_stopoutput();
01160               ESC_stopinput();
01161             }
01162           ESC_stopmbx();
01163           ESC_ALerror(ALERR_INVALIDMBXCONFIG);
01164           MBXrun = 0;
01165           ESC_ALstatus(ax);
01166           return;
01167         }
01168       ax = ESC_checkSM23(as);
01169       if ((APPstate) && (ax == (ESCpreop | ESCerror)))
01170         {
01171           ESC_stopoutput();
01172           ESC_stopinput();
01173           if( ESCvar.SMtestresult & SMRESULT_ERRSM3 )
01174             ESC_ALerror(ALERR_INVALIDINPUTSM);
01175           else
01176             ESC_ALerror(ALERR_INVALIDOUTPUTSM);
01177           ESC_ALstatus(ax);
01178           return;
01179         }
01180     }
01181   ESCvar.ALevent = 0;
01182   if ((an & ESCerror) && !(ac & ESCerror)) return; //error state not acked, leave original
01183   as = (ac << 4) | (as & 0x0f); // high bits ALcommand, low bits ALstatus
01184   switch (as)
01185     {
01186     case INIT_TO_INIT:
01187     case PREOP_TO_PREOP:
01188     case OP_TO_OP:
01189       break;
01190     case INIT_TO_PREOP:
01191       ESC_address(); // get station address
01192       an = ESC_startmbx(ac);
01193       break;
01194     case INIT_TO_BOOT:
01195       an = ESCinit | ESCerror;
01196       ESC_ALerror(ALERR_BOOTNOTSUPPORTED);
01197       break;
01198     case INIT_TO_SAFEOP:
01199     case INIT_TO_OP:
01200       an = ESCinit | ESCerror;
01201       ESC_ALerror(ALERR_INVALIDSTATECHANGE);
01202       break;
01203     case OP_TO_INIT:
01204       ESC_stopoutput();
01205     case SAFEOP_TO_INIT:
01206       ESC_stopinput();
01207     case PREOP_TO_INIT:
01208       ESC_stopmbx();
01209       an = ESCinit;
01210       break;
01211     case PREOP_TO_BOOT:
01212       an = ESCpreop | ESCerror;
01213       ESC_ALerror(ALERR_INVALIDSTATECHANGE);
01214       break;
01215     case PREOP_TO_SAFEOP:
01216     case SAFEOP_TO_SAFEOP:
01217       SM2_sml = sizeRXPDO();
01218       SM3_sml = sizeTXPDO();
01219       an = ESC_startinput(ac);
01220       if (an == ac) ESC_SMenable(2);
01221       break;
01222     case PREOP_TO_OP:
01223       an = ESCpreop | ESCerror;
01224       ESC_ALerror(ALERR_INVALIDSTATECHANGE);
01225       break;
01226     case OP_TO_PREOP:
01227       ESC_stopoutput();
01228     case SAFEOP_TO_PREOP:
01229       ESC_stopinput();
01230       an = ESCpreop;
01231       break;
01232     case SAFEOP_TO_BOOT:
01233       an = ESCsafeop | ESCerror;
01234       ESC_ALerror(ALERR_INVALIDSTATECHANGE);
01235       break;
01236     case SAFEOP_TO_OP:
01237       an = ESC_startoutput(ac);
01238       break;
01239     case OP_TO_BOOT:
01240       an = ESCsafeop | ESCerror;
01241       ESC_ALerror(ALERR_INVALIDSTATECHANGE);
01242       ESC_stopoutput();
01243       break;
01244     case OP_TO_SAFEOP:
01245       an = ESCsafeop;
01246       ESC_stopoutput();
01247       break;
01248     default :
01249       if (an == ESCop)
01250         {
01251           ESC_stopoutput();
01252           an = ESCsafeop;
01253         }
01254       if (as == ESCsafeop)
01255         ESC_stopinput();
01256       an |= ESCerror;
01257       ESC_ALerror(ALERR_UNKNOWNSTATE);
01258     }
01259   if (!(an & ESCerror) && (ESCvar.ALerror))
01260     {
01261       ESC_ALerror(ALERR_NONE); // clear error
01262     }
01263   ESC_ALstatus(an);
01264 }
01265