First Last / Mbed 2 deprecated EtherCAT-XbusMaster

EtherCAT slave that reads 3 Xsens IMU's connected to a Xbus Master

Dependencies:   MODSERIAL mbed KL25Z_ClockControl

Fork of EtherCAT by First Last

EtherCAT/esc.c@0:a8daa9348a67, 2014-11-17 (annotated)

Committer:
vsluiter
Date:
Mon Nov 17 13:55:07 2014 +0000
Revision:
0:a8daa9348a67
changed esc_hw

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:a8daa9348a67 1 /*
vsluiter 0:a8daa9348a67 2 * SOES Simple Open EtherCAT Slave
vsluiter 0:a8daa9348a67 3 *
vsluiter 0:a8daa9348a67 4 * File : esc.c
vsluiter 0:a8daa9348a67 5 * Version : 1.0.0
vsluiter 0:a8daa9348a67 6 * Date : 11-07-2010
vsluiter 0:a8daa9348a67 7 * Copyright (C) 2007-2010 Arthur Ketels
vsluiter 0:a8daa9348a67 8 *
vsluiter 0:a8daa9348a67 9 * SOES is free software; you can redistribute it and/or modify it under
vsluiter 0:a8daa9348a67 10 * the terms of the GNU General Public License version 2 as published by the Free
vsluiter 0:a8daa9348a67 11 * Software Foundation.
vsluiter 0:a8daa9348a67 12 *
vsluiter 0:a8daa9348a67 13 * SOES is distributed in the hope that it will be useful, but WITHOUT ANY
vsluiter 0:a8daa9348a67 14 * WARRANTY; without even the implied warranty of MERCHANTABILITY or
vsluiter 0:a8daa9348a67 15 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
vsluiter 0:a8daa9348a67 16 * for more details.
vsluiter 0:a8daa9348a67 17 *
vsluiter 0:a8daa9348a67 18 * As a special exception, if other files instantiate templates or use macros
vsluiter 0:a8daa9348a67 19 * or inline functions from this file, or you compile this file and link it
vsluiter 0:a8daa9348a67 20 * with other works to produce a work based on this file, this file does not
vsluiter 0:a8daa9348a67 21 * by itself cause the resulting work to be covered by the GNU General Public
vsluiter 0:a8daa9348a67 22 * License. However the source code for this file must still be made available
vsluiter 0:a8daa9348a67 23 * in accordance with section (3) of the GNU General Public License.
vsluiter 0:a8daa9348a67 24 *
vsluiter 0:a8daa9348a67 25 * This exception does not invalidate any other reasons why a work based on
vsluiter 0:a8daa9348a67 26 * this file might be covered by the GNU General Public License.
vsluiter 0:a8daa9348a67 27 *
vsluiter 0:a8daa9348a67 28 * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual
vsluiter 0:a8daa9348a67 29 * property of, and protected by Beckhoff Automation GmbH.
vsluiter 0:a8daa9348a67 30 */
vsluiter 0:a8daa9348a67 31 //#include <stdio.h>
vsluiter 0:a8daa9348a67 32
vsluiter 0:a8daa9348a67 33 #ifdef TARGET_ATMEL
vsluiter 0:a8daa9348a67 34 #include <avr/io.h>
vsluiter 0:a8daa9348a67 35 #include <avr/pgmspace.h>
vsluiter 0:a8daa9348a67 36 #else
vsluiter 0:a8daa9348a67 37 #define PROGMEM
vsluiter 0:a8daa9348a67 38 #define pgm_read_dword(x) *(x)
vsluiter 0:a8daa9348a67 39 #define pgm_read_word(x) *(x)
vsluiter 0:a8daa9348a67 40 #define pgm_read_byte(x) *(x)
vsluiter 0:a8daa9348a67 41
vsluiter 0:a8daa9348a67 42 #endif
vsluiter 0:a8daa9348a67 43
vsluiter 0:a8daa9348a67 44 #include <string.h>
vsluiter 0:a8daa9348a67 45 #include "utypes.h"
vsluiter 0:a8daa9348a67 46 #include "esc.h"
vsluiter 0:a8daa9348a67 47 #include "objectlist.h"
vsluiter 0:a8daa9348a67 48
vsluiter 0:a8daa9348a67 49 #define BITS2BYTES(b) ((b + 7) >> 3)
vsluiter 0:a8daa9348a67 50
vsluiter 0:a8daa9348a67 51 uint16 SDO_findobject(uint16 index)
vsluiter 0:a8daa9348a67 52 {
vsluiter 0:a8daa9348a67 53 int16 n = 0;
vsluiter 0:a8daa9348a67 54 while (pgm_read_word(&SDOobjects[n].index) < index)
vsluiter 0:a8daa9348a67 55 {
vsluiter 0:a8daa9348a67 56 n++;
vsluiter 0:a8daa9348a67 57 }
vsluiter 0:a8daa9348a67 58 if (pgm_read_word(&SDOobjects[n].index) != index)
vsluiter 0:a8daa9348a67 59 {
vsluiter 0:a8daa9348a67 60 return -1;
vsluiter 0:a8daa9348a67 61 }
vsluiter 0:a8daa9348a67 62 return n;
vsluiter 0:a8daa9348a67 63 }
vsluiter 0:a8daa9348a67 64
vsluiter 0:a8daa9348a67 65 uint16 sizeTXPDO(void)
vsluiter 0:a8daa9348a67 66 {
vsluiter 0:a8daa9348a67 67 uint8 c , l, si, sic;
vsluiter 0:a8daa9348a67 68 uint16 size = 0;
vsluiter 0:a8daa9348a67 69 int16 nidx;
vsluiter 0:a8daa9348a67 70 _objd FLASHSTORE *objd;
vsluiter 0:a8daa9348a67 71
vsluiter 0:a8daa9348a67 72 if (pgm_read_word(&SDO1C13[0].data))
vsluiter 0:a8daa9348a67 73 si = *((uint8 *)pgm_read_word(&SDO1C13[0].data));
vsluiter 0:a8daa9348a67 74 else
vsluiter 0:a8daa9348a67 75 si = pgm_read_byte(&SDO1C13[0].value);
vsluiter 0:a8daa9348a67 76 if (si)
vsluiter 0:a8daa9348a67 77 {
vsluiter 0:a8daa9348a67 78 for (sic = 1 ; sic <= si ; sic++)
vsluiter 0:a8daa9348a67 79 {
vsluiter 0:a8daa9348a67 80 nidx = SDO_findobject(pgm_read_word(&SDO1C13[sic].value));
vsluiter 0:a8daa9348a67 81 if (nidx > 0)
vsluiter 0:a8daa9348a67 82 {
vsluiter 0:a8daa9348a67 83 objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
vsluiter 0:a8daa9348a67 84 l = pgm_read_byte(&objd->value);
vsluiter 0:a8daa9348a67 85 for (c = 1 ; c <= l ; c++)
vsluiter 0:a8daa9348a67 86 size += (pgm_read_dword(&(objd + c)->value) & 0xff);
vsluiter 0:a8daa9348a67 87 }
vsluiter 0:a8daa9348a67 88 }
vsluiter 0:a8daa9348a67 89 }
vsluiter 0:a8daa9348a67 90 return BITS2BYTES(size);
vsluiter 0:a8daa9348a67 91 }
vsluiter 0:a8daa9348a67 92
vsluiter 0:a8daa9348a67 93 uint16 sizeRXPDO(void)
vsluiter 0:a8daa9348a67 94 {
vsluiter 0:a8daa9348a67 95 uint8 c , l, si, sic;
vsluiter 0:a8daa9348a67 96 uint16 size = 0;
vsluiter 0:a8daa9348a67 97 int16 nidx;
vsluiter 0:a8daa9348a67 98 _objd FLASHSTORE *objd;
vsluiter 0:a8daa9348a67 99
vsluiter 0:a8daa9348a67 100 if (pgm_read_word(&SDO1C12[0].data))
vsluiter 0:a8daa9348a67 101 si = *((uint8 *)pgm_read_word(&SDO1C12[0].data));
vsluiter 0:a8daa9348a67 102 else
vsluiter 0:a8daa9348a67 103 si = pgm_read_byte(&SDO1C12[0].value);
vsluiter 0:a8daa9348a67 104 if (si)
vsluiter 0:a8daa9348a67 105 {
vsluiter 0:a8daa9348a67 106 for (sic = 1 ; sic <= si ; sic++)
vsluiter 0:a8daa9348a67 107 {
vsluiter 0:a8daa9348a67 108 nidx = SDO_findobject(pgm_read_word(&SDO1C12[sic].value));
vsluiter 0:a8daa9348a67 109 if (nidx > 0)
vsluiter 0:a8daa9348a67 110 {
vsluiter 0:a8daa9348a67 111 objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
vsluiter 0:a8daa9348a67 112 l = pgm_read_byte(&objd->value);
vsluiter 0:a8daa9348a67 113 for (c = 1 ; c <= l ; c++)
vsluiter 0:a8daa9348a67 114 size += (pgm_read_dword(&(objd + c)->value) & 0xff);
vsluiter 0:a8daa9348a67 115 }
vsluiter 0:a8daa9348a67 116 }
vsluiter 0:a8daa9348a67 117 }
vsluiter 0:a8daa9348a67 118 return BITS2BYTES(size);
vsluiter 0:a8daa9348a67 119 }
vsluiter 0:a8daa9348a67 120
vsluiter 0:a8daa9348a67 121 void ESC_ALerror(uint16 errornumber)
vsluiter 0:a8daa9348a67 122 {
vsluiter 0:a8daa9348a67 123 uint16 dummy;
vsluiter 0:a8daa9348a67 124 ESCvar.ALerror = errornumber;
vsluiter 0:a8daa9348a67 125 dummy = htoes(errornumber);
vsluiter 0:a8daa9348a67 126 ESC_write(ESCREG_ALERROR, &dummy, sizeof(dummy), &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 127 }
vsluiter 0:a8daa9348a67 128
vsluiter 0:a8daa9348a67 129 void ESC_ALstatus(uint8 status)
vsluiter 0:a8daa9348a67 130 {
vsluiter 0:a8daa9348a67 131 uint16 dummy;
vsluiter 0:a8daa9348a67 132 ESCvar.ALstatus = status;
vsluiter 0:a8daa9348a67 133 dummy = htoes((uint16)status);
vsluiter 0:a8daa9348a67 134 ESC_write(ESCREG_ALSTATUS, &dummy, sizeof(dummy), &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 135 }
vsluiter 0:a8daa9348a67 136
vsluiter 0:a8daa9348a67 137 void ESC_SMack(uint8 n)
vsluiter 0:a8daa9348a67 138 {
vsluiter 0:a8daa9348a67 139 uint16 dummy;
vsluiter 0:a8daa9348a67 140 ESC_read(ESCREG_SM0STATUS + (n << 3), &dummy, 2, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 141 }
vsluiter 0:a8daa9348a67 142
vsluiter 0:a8daa9348a67 143 void ESC_SMstatus(uint8 n)
vsluiter 0:a8daa9348a67 144 {
vsluiter 0:a8daa9348a67 145 _ESCsm2 *sm;
vsluiter 0:a8daa9348a67 146 uint16 temp;
vsluiter 0:a8daa9348a67 147 sm = (_ESCsm2 *)&ESCvar.SM[n];
vsluiter 0:a8daa9348a67 148 ESC_read(ESCREG_SM0STATUS + (n << 3), &temp, 2, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 149 temp = etohs(temp);
vsluiter 0:a8daa9348a67 150 sm->ActESC = temp >> 8;
vsluiter 0:a8daa9348a67 151 sm->Status = temp;
vsluiter 0:a8daa9348a67 152 }
vsluiter 0:a8daa9348a67 153
vsluiter 0:a8daa9348a67 154 void ESC_SMwritepdi(uint8 n)
vsluiter 0:a8daa9348a67 155 {
vsluiter 0:a8daa9348a67 156 _ESCsm2 *sm;
vsluiter 0:a8daa9348a67 157 sm = (_ESCsm2 *)&ESCvar.SM[n];
vsluiter 0:a8daa9348a67 158 ESC_write(ESCREG_SM0PDI + (n << 3), &(sm->ActPDI), 1, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 159 }
vsluiter 0:a8daa9348a67 160
vsluiter 0:a8daa9348a67 161 void ESC_SMenable(uint8 n)
vsluiter 0:a8daa9348a67 162 {
vsluiter 0:a8daa9348a67 163 _ESCsm2 *sm;
vsluiter 0:a8daa9348a67 164 sm = (_ESCsm2 *)&ESCvar.SM[n];
vsluiter 0:a8daa9348a67 165 sm->ActPDI &= ~ESCREG_SMENABLE_BIT;
vsluiter 0:a8daa9348a67 166 ESC_SMwritepdi(n);
vsluiter 0:a8daa9348a67 167 }
vsluiter 0:a8daa9348a67 168
vsluiter 0:a8daa9348a67 169 void ESC_SMdisable(uint8 n)
vsluiter 0:a8daa9348a67 170 {
vsluiter 0:a8daa9348a67 171 _ESCsm2 *sm;
vsluiter 0:a8daa9348a67 172 sm = (_ESCsm2 *)&ESCvar.SM[n];
vsluiter 0:a8daa9348a67 173 sm->ActPDI |= ESCREG_SMENABLE_BIT;
vsluiter 0:a8daa9348a67 174 ESC_SMwritepdi(n);
vsluiter 0:a8daa9348a67 175 }
vsluiter 0:a8daa9348a67 176
vsluiter 0:a8daa9348a67 177 void ESC_address(void)
vsluiter 0:a8daa9348a67 178 {
vsluiter 0:a8daa9348a67 179 ESC_read(ESCREG_ADDRESS, &ESCvar.address, sizeof(ESCvar.address), &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 180 ESCvar.address = etohs(ESCvar.address);
vsluiter 0:a8daa9348a67 181 }
vsluiter 0:a8daa9348a67 182
vsluiter 0:a8daa9348a67 183 uint8 ESC_WDstatus(void)
vsluiter 0:a8daa9348a67 184 {
vsluiter 0:a8daa9348a67 185 uint16 wdstatus;
vsluiter 0:a8daa9348a67 186 ESC_read(ESCREG_WDSTATUS, &wdstatus, 2, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 187 wdstatus = etohs(wdstatus);
vsluiter 0:a8daa9348a67 188 return (uint8)wdstatus;
vsluiter 0:a8daa9348a67 189 }
vsluiter 0:a8daa9348a67 190
vsluiter 0:a8daa9348a67 191 uint8 ESC_checkmbx(uint8 state)
vsluiter 0:a8daa9348a67 192 {
vsluiter 0:a8daa9348a67 193 _ESCsm2 *SM;
vsluiter 0:a8daa9348a67 194 ESC_read(ESCREG_SM0, &ESCvar.SM[0], sizeof(ESCvar.SM[0]), &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 195 ESC_read(ESCREG_SM1, &ESCvar.SM[1], sizeof(ESCvar.SM[1]), &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 196 SM = (_ESCsm2 *)&ESCvar.SM[0];
vsluiter 0:a8daa9348a67 197 if ((etohs(SM->PSA) != MBX0_sma) || (etohs(SM->Length) != MBX0_sml) || (SM->Command != MBX0_smc) || (ESCvar.SM[0].ECsm == 0))
vsluiter 0:a8daa9348a67 198 {
vsluiter 0:a8daa9348a67 199 ESCvar.SMtestresult = SMRESULT_ERRSM0;
vsluiter 0:a8daa9348a67 200 ESC_SMdisable(0);
vsluiter 0:a8daa9348a67 201 ESC_SMdisable(1);
vsluiter 0:a8daa9348a67 202 return (uint8)(ESCinit | ESCerror); //fail state change
vsluiter 0:a8daa9348a67 203 }
vsluiter 0:a8daa9348a67 204 SM = (_ESCsm2 *)&ESCvar.SM[1];
vsluiter 0:a8daa9348a67 205 if ((etohs(SM->PSA) != MBX1_sma) || (etohs(SM->Length) != MBX1_sml) || (SM->Command != MBX1_smc) || (ESCvar.SM[1].ECsm == 0))
vsluiter 0:a8daa9348a67 206 {
vsluiter 0:a8daa9348a67 207 ESCvar.SMtestresult = SMRESULT_ERRSM1;
vsluiter 0:a8daa9348a67 208 ESC_SMdisable(0);
vsluiter 0:a8daa9348a67 209 ESC_SMdisable(1);
vsluiter 0:a8daa9348a67 210 return ESCinit | ESCerror; //fail state change
vsluiter 0:a8daa9348a67 211 }
vsluiter 0:a8daa9348a67 212 return state;
vsluiter 0:a8daa9348a67 213 }
vsluiter 0:a8daa9348a67 214
vsluiter 0:a8daa9348a67 215 uint8 ESC_startmbx(uint8 state)
vsluiter 0:a8daa9348a67 216 {
vsluiter 0:a8daa9348a67 217 ESC_SMenable(0);
vsluiter 0:a8daa9348a67 218 ESC_SMenable(1);
vsluiter 0:a8daa9348a67 219 ESC_SMstatus(0);
vsluiter 0:a8daa9348a67 220 ESC_SMstatus(1);
vsluiter 0:a8daa9348a67 221 if ((state = ESC_checkmbx(state)) & ESCerror)
vsluiter 0:a8daa9348a67 222 {
vsluiter 0:a8daa9348a67 223 ESC_ALerror(ALERR_INVALIDMBXCONFIG);
vsluiter 0:a8daa9348a67 224 MBXrun = 0;
vsluiter 0:a8daa9348a67 225 }
vsluiter 0:a8daa9348a67 226 else
vsluiter 0:a8daa9348a67 227 {
vsluiter 0:a8daa9348a67 228 ESCvar.toggle = ESCvar.SM[1].ECrep; //sync repeat request toggle state
vsluiter 0:a8daa9348a67 229 MBXrun = 1;
vsluiter 0:a8daa9348a67 230 }
vsluiter 0:a8daa9348a67 231 return state;
vsluiter 0:a8daa9348a67 232 }
vsluiter 0:a8daa9348a67 233
vsluiter 0:a8daa9348a67 234 void ESC_stopmbx(void)
vsluiter 0:a8daa9348a67 235 {
vsluiter 0:a8daa9348a67 236 uint8 n;
vsluiter 0:a8daa9348a67 237 MBXrun = 0;
vsluiter 0:a8daa9348a67 238 ESC_SMdisable(0);
vsluiter 0:a8daa9348a67 239 ESC_SMdisable(1);
vsluiter 0:a8daa9348a67 240 for (n = 0 ; n < MBXBUFFERS ; n++) MBXcontrol[n].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 241 ESCvar.mbxoutpost = 0;
vsluiter 0:a8daa9348a67 242 ESCvar.mbxbackup = 0;
vsluiter 0:a8daa9348a67 243 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 244 ESCvar.mbxfree = 1;
vsluiter 0:a8daa9348a67 245 ESCvar.toggle = 0;
vsluiter 0:a8daa9348a67 246 ESCvar.mbxincnt = 0;
vsluiter 0:a8daa9348a67 247 ESCvar.segmented = 0;
vsluiter 0:a8daa9348a67 248 ESCvar.frags = 0;
vsluiter 0:a8daa9348a67 249 ESCvar.fragsleft = 0;
vsluiter 0:a8daa9348a67 250 ESCvar.txcue = 0;
vsluiter 0:a8daa9348a67 251 }
vsluiter 0:a8daa9348a67 252
vsluiter 0:a8daa9348a67 253 void ESC_readmbx(void)
vsluiter 0:a8daa9348a67 254 {
vsluiter 0:a8daa9348a67 255 _MBX *MB = &MBX[0];
vsluiter 0:a8daa9348a67 256 uint16 length;
vsluiter 0:a8daa9348a67 257 ESC_read(MBX0_sma, MB,MBXHSIZE, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 258 length = etohs(MB->header.length);
vsluiter 0:a8daa9348a67 259 if (length > (MBX0_sml - MBXHSIZE)) length = MBX0_sml - MBXHSIZE;
vsluiter 0:a8daa9348a67 260 ESC_read(MBX0_sma + MBXHSIZE, &(MB->b[0]), length, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 261 if (length + MBXHSIZE < MBX0_sml)
vsluiter 0:a8daa9348a67 262 {
vsluiter 0:a8daa9348a67 263 ESC_read(MBX0_sme, &length, 1, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 264 }
vsluiter 0:a8daa9348a67 265 MBXcontrol[0].state = MBXstate_inclaim;
vsluiter 0:a8daa9348a67 266 }
vsluiter 0:a8daa9348a67 267
vsluiter 0:a8daa9348a67 268 void ESC_writembx(uint8 n)
vsluiter 0:a8daa9348a67 269 {
vsluiter 0:a8daa9348a67 270 _MBX *MB = &MBX[n];
vsluiter 0:a8daa9348a67 271 uint8 dummy = 0;
vsluiter 0:a8daa9348a67 272 uint16 length;
vsluiter 0:a8daa9348a67 273 length = etohs(MB->header.length);
vsluiter 0:a8daa9348a67 274 if (length > (MBX1_sml - MBXHSIZE)) length = MBX1_sml - MBXHSIZE;
vsluiter 0:a8daa9348a67 275 ESC_write(MBX1_sma, MB, MBXHSIZE + length, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 276 if (length + MBXHSIZE < MBX1_sml)
vsluiter 0:a8daa9348a67 277 {
vsluiter 0:a8daa9348a67 278 ESC_write(MBX1_sme, &dummy, 1, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 279 }
vsluiter 0:a8daa9348a67 280 ESCvar.mbxfree = 0;
vsluiter 0:a8daa9348a67 281 }
vsluiter 0:a8daa9348a67 282
vsluiter 0:a8daa9348a67 283 void ESC_ackmbxread(void)
vsluiter 0:a8daa9348a67 284 {
vsluiter 0:a8daa9348a67 285 uint8 dummy = 0;
vsluiter 0:a8daa9348a67 286 ESC_write(MBX1_sma, &dummy, 1, &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 287 ESCvar.mbxfree = 1;
vsluiter 0:a8daa9348a67 288 }
vsluiter 0:a8daa9348a67 289
vsluiter 0:a8daa9348a67 290 uint8 ESC_claimbuffer(void)
vsluiter 0:a8daa9348a67 291 {
vsluiter 0:a8daa9348a67 292 _MBX *MB;
vsluiter 0:a8daa9348a67 293 uint8 n = MBXBUFFERS - 1;
vsluiter 0:a8daa9348a67 294 while ((n > 0) && (MBXcontrol[n].state)) n--;
vsluiter 0:a8daa9348a67 295 if (n)
vsluiter 0:a8daa9348a67 296 {
vsluiter 0:a8daa9348a67 297 MBXcontrol[n].state = MBXstate_outclaim;
vsluiter 0:a8daa9348a67 298 MB = &MBX[n];
vsluiter 0:a8daa9348a67 299 ESCvar.mbxcnt = (++ESCvar.mbxcnt) & 0x07;
vsluiter 0:a8daa9348a67 300 if (ESCvar.mbxcnt == 0) ESCvar.mbxcnt = 1;
vsluiter 0:a8daa9348a67 301 MB->header.address = htoes(0x0000); // destination is master
vsluiter 0:a8daa9348a67 302 MB->header.channel = 0;
vsluiter 0:a8daa9348a67 303 MB->header.priority = 0;
vsluiter 0:a8daa9348a67 304 MB->header.mbxcnt = ESCvar.mbxcnt;
vsluiter 0:a8daa9348a67 305 ESCvar.txcue++;
vsluiter 0:a8daa9348a67 306 }
vsluiter 0:a8daa9348a67 307 return n;
vsluiter 0:a8daa9348a67 308 }
vsluiter 0:a8daa9348a67 309
vsluiter 0:a8daa9348a67 310 uint8 ESC_outreqbuffer(void)
vsluiter 0:a8daa9348a67 311 {
vsluiter 0:a8daa9348a67 312 uint8 n = MBXBUFFERS-1;
vsluiter 0:a8daa9348a67 313 while ((n > 0) && (MBXcontrol[n].state != MBXstate_outreq)) n--;
vsluiter 0:a8daa9348a67 314 return n;
vsluiter 0:a8daa9348a67 315 }
vsluiter 0:a8daa9348a67 316
vsluiter 0:a8daa9348a67 317 void MBX_error(uint16 error)
vsluiter 0:a8daa9348a67 318 {
vsluiter 0:a8daa9348a67 319 uint8 MBXout;
vsluiter 0:a8daa9348a67 320 _MBXerr *mbxerr;
vsluiter 0:a8daa9348a67 321 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 322 if (MBXout)
vsluiter 0:a8daa9348a67 323 {
vsluiter 0:a8daa9348a67 324 mbxerr = (_MBXerr *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 325 mbxerr->mbxheader.length = htoes((uint16)0x04);
vsluiter 0:a8daa9348a67 326 mbxerr->mbxheader.mbxtype = MBXERR;
vsluiter 0:a8daa9348a67 327 mbxerr->type = htoes((uint16)0x01);
vsluiter 0:a8daa9348a67 328 mbxerr->detail = htoes(error);
vsluiter 0:a8daa9348a67 329 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 330 }
vsluiter 0:a8daa9348a67 331 }
vsluiter 0:a8daa9348a67 332
vsluiter 0:a8daa9348a67 333 uint8 ESC_mbxprocess(void)
vsluiter 0:a8daa9348a67 334 {
vsluiter 0:a8daa9348a67 335 uint8 mbxhandle = 0;
vsluiter 0:a8daa9348a67 336 _MBX *MB = &MBX[0];
vsluiter 0:a8daa9348a67 337
vsluiter 0:a8daa9348a67 338 if (!MBXrun)
vsluiter 0:a8daa9348a67 339 {
vsluiter 0:a8daa9348a67 340 return 0; //nothing to do
vsluiter 0:a8daa9348a67 341 }
vsluiter 0:a8daa9348a67 342
vsluiter 0:a8daa9348a67 343 if (ESCvar.ALevent & ESCREG_ALEVENT_SM_MASK) // SM0/1 access or SMn change event
vsluiter 0:a8daa9348a67 344 {
vsluiter 0:a8daa9348a67 345 ESC_SMstatus(0);
vsluiter 0:a8daa9348a67 346 ESC_SMstatus(1);
vsluiter 0:a8daa9348a67 347 }
vsluiter 0:a8daa9348a67 348
vsluiter 0:a8daa9348a67 349 if (ESCvar.mbxoutpost && ESCvar.SM[1].IntR) //outmbx read by master
vsluiter 0:a8daa9348a67 350 {
vsluiter 0:a8daa9348a67 351 ESC_ackmbxread();
vsluiter 0:a8daa9348a67 352 if (ESCvar.mbxbackup) // dispose old backup
vsluiter 0:a8daa9348a67 353 {
vsluiter 0:a8daa9348a67 354 MBXcontrol[ESCvar.mbxbackup].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 355 }
vsluiter 0:a8daa9348a67 356 if (MBXcontrol[ESCvar.mbxoutpost].state == MBXstate_again) // if still to do
vsluiter 0:a8daa9348a67 357 {
vsluiter 0:a8daa9348a67 358 ESC_writembx(ESCvar.mbxoutpost);
vsluiter 0:a8daa9348a67 359 }
vsluiter 0:a8daa9348a67 360 MBXcontrol[ESCvar.mbxoutpost].state = MBXstate_backup; //create new backup
vsluiter 0:a8daa9348a67 361 ESCvar.mbxbackup = ESCvar.mbxoutpost;
vsluiter 0:a8daa9348a67 362 ESCvar.mbxoutpost = 0;
vsluiter 0:a8daa9348a67 363 return 0;
vsluiter 0:a8daa9348a67 364 }
vsluiter 0:a8daa9348a67 365
vsluiter 0:a8daa9348a67 366 if (ESCvar.SM[1].ECrep != ESCvar.toggle) // repeat request
vsluiter 0:a8daa9348a67 367 {
vsluiter 0:a8daa9348a67 368 if (ESCvar.mbxoutpost || ESCvar.mbxbackup)
vsluiter 0:a8daa9348a67 369 {
vsluiter 0:a8daa9348a67 370 if (!ESCvar.mbxoutpost) // if outmbx empty
vsluiter 0:a8daa9348a67 371 {
vsluiter 0:a8daa9348a67 372 ESC_writembx(ESCvar.mbxbackup); // use backup mbx
vsluiter 0:a8daa9348a67 373 }
vsluiter 0:a8daa9348a67 374 else
vsluiter 0:a8daa9348a67 375 {
vsluiter 0:a8daa9348a67 376 ESC_SMdisable(1); // reset mailbox
vsluiter 0:a8daa9348a67 377 MBXcontrol[ESCvar.mbxoutpost].state = MBXstate_again; // have to resend later
vsluiter 0:a8daa9348a67 378 ESC_SMenable(1); // activate mailbox
vsluiter 0:a8daa9348a67 379 ESC_writembx(ESCvar.mbxbackup); // use backup mbx
vsluiter 0:a8daa9348a67 380 }
vsluiter 0:a8daa9348a67 381 ESCvar.toggle = ESCvar.SM[1].ECrep;
vsluiter 0:a8daa9348a67 382 ESCvar.SM[1].PDIrep = ESCvar.toggle;
vsluiter 0:a8daa9348a67 383 ESC_SMwritepdi(1);
vsluiter 0:a8daa9348a67 384 }
vsluiter 0:a8daa9348a67 385 return 0;
vsluiter 0:a8daa9348a67 386 }
vsluiter 0:a8daa9348a67 387
vsluiter 0:a8daa9348a67 388 // if the outmailbox is free check if we have something to send
vsluiter 0:a8daa9348a67 389 if (ESCvar.txcue && (ESCvar.mbxfree || !ESCvar.SM[1].MBXstat))
vsluiter 0:a8daa9348a67 390 {
vsluiter 0:a8daa9348a67 391 mbxhandle = ESC_outreqbuffer(); // check out request mbx
vsluiter 0:a8daa9348a67 392 if (mbxhandle) // outmbx empty and outreq mbx available
vsluiter 0:a8daa9348a67 393 {
vsluiter 0:a8daa9348a67 394 ESC_writembx(mbxhandle);
vsluiter 0:a8daa9348a67 395 MBXcontrol[mbxhandle].state = MBXstate_outpost; // change state
vsluiter 0:a8daa9348a67 396 ESCvar.mbxoutpost = mbxhandle;
vsluiter 0:a8daa9348a67 397 if (ESCvar.txcue) ESCvar.txcue--;
vsluiter 0:a8daa9348a67 398 }
vsluiter 0:a8daa9348a67 399 }
vsluiter 0:a8daa9348a67 400
vsluiter 0:a8daa9348a67 401 // read mailbox if full and no xoe in progress
vsluiter 0:a8daa9348a67 402 if (ESCvar.SM[0].MBXstat && !MBXcontrol[0].state && !ESCvar.mbxoutpost && !ESCvar.xoe)
vsluiter 0:a8daa9348a67 403 {
vsluiter 0:a8daa9348a67 404 ESC_readmbx();
vsluiter 0:a8daa9348a67 405 ESCvar.SM[0].MBXstat = 0;
vsluiter 0:a8daa9348a67 406 if (etohs(MB->header.length) < 8)
vsluiter 0:a8daa9348a67 407 {
vsluiter 0:a8daa9348a67 408 if (etohs(MB->header.length) == 0)
vsluiter 0:a8daa9348a67 409 MBX_error(MBXERR_INVALIDHEADER);
vsluiter 0:a8daa9348a67 410 else
vsluiter 0:a8daa9348a67 411 MBX_error(MBXERR_SIZETOOSHORT);
vsluiter 0:a8daa9348a67 412 MBXcontrol[0].state = MBXstate_idle; // drop mailbox
vsluiter 0:a8daa9348a67 413 }
vsluiter 0:a8daa9348a67 414 if ((MB->header.mbxcnt !=0) && (MB->header.mbxcnt == ESCvar.mbxincnt))
vsluiter 0:a8daa9348a67 415 {
vsluiter 0:a8daa9348a67 416 MBXcontrol[0].state = MBXstate_idle; // drop mailbox
vsluiter 0:a8daa9348a67 417 }
vsluiter 0:a8daa9348a67 418 ESCvar.mbxincnt = MB->header.mbxcnt;
vsluiter 0:a8daa9348a67 419 return 1;
vsluiter 0:a8daa9348a67 420 }
vsluiter 0:a8daa9348a67 421
vsluiter 0:a8daa9348a67 422 if (ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE) // ack changes in non used SM
vsluiter 0:a8daa9348a67 423 {
vsluiter 0:a8daa9348a67 424 ESC_SMack(4);
vsluiter 0:a8daa9348a67 425 ESC_SMack(5);
vsluiter 0:a8daa9348a67 426 ESC_SMack(6);
vsluiter 0:a8daa9348a67 427 ESC_SMack(7);
vsluiter 0:a8daa9348a67 428 }
vsluiter 0:a8daa9348a67 429
vsluiter 0:a8daa9348a67 430 return 0;
vsluiter 0:a8daa9348a67 431 }
vsluiter 0:a8daa9348a67 432
vsluiter 0:a8daa9348a67 433 int16 SDO_findsubindex(int16 nidx, uint8 subindex)
vsluiter 0:a8daa9348a67 434 {
vsluiter 0:a8daa9348a67 435 _objd FLASHSTORE *objd;
vsluiter 0:a8daa9348a67 436 int16 n = 0;
vsluiter 0:a8daa9348a67 437 uint8 maxsub;
vsluiter 0:a8daa9348a67 438 objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
vsluiter 0:a8daa9348a67 439 maxsub = pgm_read_byte(&SDOobjects[nidx].maxsub);
vsluiter 0:a8daa9348a67 440 while (( pgm_read_byte(&(objd + n)->subindex) < subindex) && (n < maxsub))
vsluiter 0:a8daa9348a67 441 {
vsluiter 0:a8daa9348a67 442 n++;
vsluiter 0:a8daa9348a67 443 }
vsluiter 0:a8daa9348a67 444 if (pgm_read_byte(&(objd + n)->subindex) != subindex)
vsluiter 0:a8daa9348a67 445 {
vsluiter 0:a8daa9348a67 446 return -1;
vsluiter 0:a8daa9348a67 447 }
vsluiter 0:a8daa9348a67 448 return n;
vsluiter 0:a8daa9348a67 449 }
vsluiter 0:a8daa9348a67 450
vsluiter 0:a8daa9348a67 451 void copy2mbx(void *source, void *dest, uint16 size)
vsluiter 0:a8daa9348a67 452 {
vsluiter 0:a8daa9348a67 453 memcpy(dest, source, size);
vsluiter 0:a8daa9348a67 454 }
vsluiter 0:a8daa9348a67 455
vsluiter 0:a8daa9348a67 456 void SDO_abort(uint16 index, uint8 subindex, uint32 abortcode)
vsluiter 0:a8daa9348a67 457 {
vsluiter 0:a8daa9348a67 458 uint8 MBXout;
vsluiter 0:a8daa9348a67 459 _COEsdo *coeres;
vsluiter 0:a8daa9348a67 460 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 461 if (MBXout)
vsluiter 0:a8daa9348a67 462 {
vsluiter 0:a8daa9348a67 463 coeres = (_COEsdo *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 464 coeres->mbxheader.length = htoes(COE_DEFAULTLENGTH);
vsluiter 0:a8daa9348a67 465 coeres->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 466 coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOREQUEST << 12));
vsluiter 0:a8daa9348a67 467 coeres->index = htoes(index);
vsluiter 0:a8daa9348a67 468 coeres->subindex = subindex;
vsluiter 0:a8daa9348a67 469 coeres->command = COE_COMMAND_SDOABORT;
vsluiter 0:a8daa9348a67 470 coeres->size = htoel(abortcode);
vsluiter 0:a8daa9348a67 471 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 472 }
vsluiter 0:a8daa9348a67 473 }
vsluiter 0:a8daa9348a67 474
vsluiter 0:a8daa9348a67 475 void SDO_upload(void)
vsluiter 0:a8daa9348a67 476 {
vsluiter 0:a8daa9348a67 477 _COEsdo *coesdo,*coeres;
vsluiter 0:a8daa9348a67 478 uint16 index;
vsluiter 0:a8daa9348a67 479 uint8 subindex;
vsluiter 0:a8daa9348a67 480 int16 nidx, nsub;
vsluiter 0:a8daa9348a67 481 uint8 MBXout;
vsluiter 0:a8daa9348a67 482 uint16 size;
vsluiter 0:a8daa9348a67 483 uint8 dss;
vsluiter 0:a8daa9348a67 484 _objd FLASHSTORE *objd;
vsluiter 0:a8daa9348a67 485 coesdo = (_COEsdo *)&MBX[0];
vsluiter 0:a8daa9348a67 486 index = etohs(coesdo->index);
vsluiter 0:a8daa9348a67 487 subindex = coesdo->subindex;
vsluiter 0:a8daa9348a67 488 nidx = SDO_findobject(index);
vsluiter 0:a8daa9348a67 489 if (nidx >= 0)
vsluiter 0:a8daa9348a67 490 {
vsluiter 0:a8daa9348a67 491 nsub = SDO_findsubindex(nidx, subindex);
vsluiter 0:a8daa9348a67 492 if (nsub >= 0)
vsluiter 0:a8daa9348a67 493 {
vsluiter 0:a8daa9348a67 494 objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
vsluiter 0:a8daa9348a67 495 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 496 if (MBXout)
vsluiter 0:a8daa9348a67 497 {
vsluiter 0:a8daa9348a67 498 coeres = (_COEsdo *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 499 coeres->mbxheader.length = htoes(COE_DEFAULTLENGTH);
vsluiter 0:a8daa9348a67 500 coeres->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 501 coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDORESPONSE << 12));
vsluiter 0:a8daa9348a67 502 size = pgm_read_word(&(objd + nsub)->bitlength);
vsluiter 0:a8daa9348a67 503 // expedited bits used calculation
vsluiter 0:a8daa9348a67 504 dss = 0x0c;
vsluiter 0:a8daa9348a67 505 if (size>8) dss = 0x08;
vsluiter 0:a8daa9348a67 506 if (size>16) dss = 0x04;
vsluiter 0:a8daa9348a67 507 if (size>24) dss = 0x00;
vsluiter 0:a8daa9348a67 508 coeres->index = htoes(index);
vsluiter 0:a8daa9348a67 509 coeres->subindex = subindex;
vsluiter 0:a8daa9348a67 510 if (size <= 32)
vsluiter 0:a8daa9348a67 511 { // expedited response i.e. length<=4 bytes
vsluiter 0:a8daa9348a67 512 coeres->command = COE_COMMAND_UPLOADRESPONSE +
vsluiter 0:a8daa9348a67 513 COE_SIZE_INDICATOR +
vsluiter 0:a8daa9348a67 514 COE_EXPEDITED_INDICATOR + dss;
vsluiter 0:a8daa9348a67 515 if (pgm_read_word(&(objd+nsub)->data) == nil)
vsluiter 0:a8daa9348a67 516 {
vsluiter 0:a8daa9348a67 517 coeres->size = htoel(pgm_read_dword(&(objd + nsub)->value)); //use constant value
vsluiter 0:a8daa9348a67 518 }
vsluiter 0:a8daa9348a67 519 else
vsluiter 0:a8daa9348a67 520 {
vsluiter 0:a8daa9348a67 521 size = (size + 7) >> 3; //convert bits to bytes
vsluiter 0:a8daa9348a67 522 copy2mbx((void *)pgm_read_word(&(objd + nsub)->data), &(coeres->size), size); //use dynamic data
vsluiter 0:a8daa9348a67 523 }
vsluiter 0:a8daa9348a67 524 }
vsluiter 0:a8daa9348a67 525 else
vsluiter 0:a8daa9348a67 526 { // normal response i.e. length>4 bytes
vsluiter 0:a8daa9348a67 527 coeres->command = COE_COMMAND_UPLOADRESPONSE +
vsluiter 0:a8daa9348a67 528 COE_SIZE_INDICATOR;
vsluiter 0:a8daa9348a67 529 size=(size + 7) >> 3; //convert bits to bytes
vsluiter 0:a8daa9348a67 530 coeres->size = htoel(size);
vsluiter 0:a8daa9348a67 531 if ((size + COE_HEADERSIZE) > MBXDSIZE)
vsluiter 0:a8daa9348a67 532 { // segemented transfer needed
vsluiter 0:a8daa9348a67 533 ESCvar.frags = size; // set total size in bytes
vsluiter 0:a8daa9348a67 534 size = MBXDSIZE - COE_HEADERSIZE; //limit to mailbox size
vsluiter 0:a8daa9348a67 535 ESCvar.fragsleft = size; // number of bytes done
vsluiter 0:a8daa9348a67 536 ESCvar.segmented = MBXSEU; // signal segmented transfer
vsluiter 0:a8daa9348a67 537 ESCvar.data = (void *)pgm_read_word(&(objd + nsub)->data);
vsluiter 0:a8daa9348a67 538 }
vsluiter 0:a8daa9348a67 539 else
vsluiter 0:a8daa9348a67 540 {
vsluiter 0:a8daa9348a67 541 ESCvar.segmented = 0;
vsluiter 0:a8daa9348a67 542 }
vsluiter 0:a8daa9348a67 543 coeres->mbxheader.length = htoes(COE_HEADERSIZE + size);
vsluiter 0:a8daa9348a67 544 copy2mbx((void *)pgm_read_word(&(objd + nsub)->data), (&(coeres->size)) + 1, size); //use dynamic data
vsluiter 0:a8daa9348a67 545 }
vsluiter 0:a8daa9348a67 546 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 547 }
vsluiter 0:a8daa9348a67 548 }
vsluiter 0:a8daa9348a67 549 else
vsluiter 0:a8daa9348a67 550 {
vsluiter 0:a8daa9348a67 551 SDO_abort(index, subindex, ABORT_NOSUBINDEX);
vsluiter 0:a8daa9348a67 552 }
vsluiter 0:a8daa9348a67 553 }
vsluiter 0:a8daa9348a67 554 else
vsluiter 0:a8daa9348a67 555 {
vsluiter 0:a8daa9348a67 556 SDO_abort(index, subindex, ABORT_NOOBJECT);
vsluiter 0:a8daa9348a67 557 }
vsluiter 0:a8daa9348a67 558 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 559 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 560 }
vsluiter 0:a8daa9348a67 561
vsluiter 0:a8daa9348a67 562
vsluiter 0:a8daa9348a67 563 void SDO_uploadsegment(void)
vsluiter 0:a8daa9348a67 564 {
vsluiter 0:a8daa9348a67 565 _COEsdo *coesdo, *coeres;
vsluiter 0:a8daa9348a67 566 uint8 MBXout;
vsluiter 0:a8daa9348a67 567 uint16 size, offset;
vsluiter 0:a8daa9348a67 568 coesdo = (_COEsdo *)&MBX[0];
vsluiter 0:a8daa9348a67 569 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 570 if (MBXout)
vsluiter 0:a8daa9348a67 571 {
vsluiter 0:a8daa9348a67 572 coeres = (_COEsdo *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 573 offset = ESCvar.fragsleft;
vsluiter 0:a8daa9348a67 574 size = ESCvar.frags - ESCvar.fragsleft;
vsluiter 0:a8daa9348a67 575 coeres->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 576 coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDORESPONSE << 12));
vsluiter 0:a8daa9348a67 577 coeres->command = COE_COMMAND_UPLOADSEGMENT +
vsluiter 0:a8daa9348a67 578 (coesdo->command & COE_TOGGLEBIT); // copy toggle bit
vsluiter 0:a8daa9348a67 579 if ((size + COE_SEGMENTHEADERSIZE) > MBXDSIZE)
vsluiter 0:a8daa9348a67 580 { // more segemented transfer needed
vsluiter 0:a8daa9348a67 581 size = MBXDSIZE - COE_SEGMENTHEADERSIZE; //limit to mailbox size
vsluiter 0:a8daa9348a67 582 ESCvar.fragsleft += size; // number of bytes done
vsluiter 0:a8daa9348a67 583 coeres->mbxheader.length = htoes(COE_SEGMENTHEADERSIZE + size);
vsluiter 0:a8daa9348a67 584 }
vsluiter 0:a8daa9348a67 585 else
vsluiter 0:a8daa9348a67 586 {// last segment
vsluiter 0:a8daa9348a67 587 ESCvar.segmented = 0;
vsluiter 0:a8daa9348a67 588 ESCvar.frags = 0;
vsluiter 0:a8daa9348a67 589 ESCvar.fragsleft = 0;
vsluiter 0:a8daa9348a67 590 coeres->command += COE_COMMAND_LASTSEGMENTBIT;
vsluiter 0:a8daa9348a67 591 if (size >= 7)
vsluiter 0:a8daa9348a67 592 {
vsluiter 0:a8daa9348a67 593 coeres->mbxheader.length = htoes(COE_SEGMENTHEADERSIZE + size);
vsluiter 0:a8daa9348a67 594 }
vsluiter 0:a8daa9348a67 595 else
vsluiter 0:a8daa9348a67 596 {
vsluiter 0:a8daa9348a67 597 coeres->command += (7 - size) << 1;
vsluiter 0:a8daa9348a67 598 coeres->mbxheader.length = htoes(COE_DEFAULTLENGTH);
vsluiter 0:a8daa9348a67 599 }
vsluiter 0:a8daa9348a67 600 }
vsluiter 0:a8daa9348a67 601 copy2mbx((uint8*)ESCvar.data + offset, (&(coeres->command)) + 1, size); //copy to mailbox
vsluiter 0:a8daa9348a67 602
vsluiter 0:a8daa9348a67 603 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 604 }
vsluiter 0:a8daa9348a67 605 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 606 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 607 }
vsluiter 0:a8daa9348a67 608
vsluiter 0:a8daa9348a67 609 void SDO_download(void)
vsluiter 0:a8daa9348a67 610 {
vsluiter 0:a8daa9348a67 611 _COEsdo *coesdo, *coeres;
vsluiter 0:a8daa9348a67 612 uint16 index;
vsluiter 0:a8daa9348a67 613 uint8 subindex;
vsluiter 0:a8daa9348a67 614 int16 nidx, nsub;
vsluiter 0:a8daa9348a67 615 uint8 MBXout;
vsluiter 0:a8daa9348a67 616 uint16 size, actsize;
vsluiter 0:a8daa9348a67 617 _objd FLASHSTORE *objd;
vsluiter 0:a8daa9348a67 618 uint32 *mbxdata;
vsluiter 0:a8daa9348a67 619 coesdo = (_COEsdo *)&MBX[0];
vsluiter 0:a8daa9348a67 620 index = etohs(coesdo->index);
vsluiter 0:a8daa9348a67 621 subindex = coesdo->subindex;
vsluiter 0:a8daa9348a67 622 nidx = SDO_findobject(index);
vsluiter 0:a8daa9348a67 623 if (nidx >= 0)
vsluiter 0:a8daa9348a67 624 {
vsluiter 0:a8daa9348a67 625 nsub = SDO_findsubindex(nidx, subindex);
vsluiter 0:a8daa9348a67 626 if (nsub >= 0)
vsluiter 0:a8daa9348a67 627 {
vsluiter 0:a8daa9348a67 628 objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
vsluiter 0:a8daa9348a67 629 if ((pgm_read_word(&(objd + nsub)->access) == ATYPE_RW) ||
vsluiter 0:a8daa9348a67 630 ((pgm_read_word(&(objd + nsub)->access) == ATYPE_RWpre) && ((ESCvar.ALstatus & 0x0f) == ESCpreop)))
vsluiter 0:a8daa9348a67 631 {
vsluiter 0:a8daa9348a67 632 if (coesdo->command & COE_EXPEDITED_INDICATOR) //expedited?
vsluiter 0:a8daa9348a67 633 {
vsluiter 0:a8daa9348a67 634 size = 4 - ((coesdo->command & 0x0c) >> 2);
vsluiter 0:a8daa9348a67 635 mbxdata = &(coesdo->size);
vsluiter 0:a8daa9348a67 636 }
vsluiter 0:a8daa9348a67 637 else // normal upload
vsluiter 0:a8daa9348a67 638 {
vsluiter 0:a8daa9348a67 639 size = (etohs(coesdo->size) & 0xffff);
vsluiter 0:a8daa9348a67 640 mbxdata = (&(coesdo->size)) + 1;
vsluiter 0:a8daa9348a67 641 }
vsluiter 0:a8daa9348a67 642 actsize = (pgm_read_word(&(objd + nsub)->bitlength) + 7) >> 3;
vsluiter 0:a8daa9348a67 643 if ( actsize == size )
vsluiter 0:a8daa9348a67 644 {
vsluiter 0:a8daa9348a67 645 copy2mbx(mbxdata, (void *)pgm_read_word(&(objd + nsub)->data), size);
vsluiter 0:a8daa9348a67 646 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 647 if (MBXout)
vsluiter 0:a8daa9348a67 648 {
vsluiter 0:a8daa9348a67 649 coeres = (_COEsdo *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 650 coeres->mbxheader.length = htoes(COE_DEFAULTLENGTH);
vsluiter 0:a8daa9348a67 651 coeres->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 652 coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDORESPONSE << 12));
vsluiter 0:a8daa9348a67 653 coeres->index = htoes(index);
vsluiter 0:a8daa9348a67 654 coeres->subindex = subindex;
vsluiter 0:a8daa9348a67 655 coeres->command = COE_COMMAND_DOWNLOADRESPONSE;
vsluiter 0:a8daa9348a67 656 coeres->size = htoel(0);
vsluiter 0:a8daa9348a67 657 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 658 }
vsluiter 0:a8daa9348a67 659 // external object write handler
vsluiter 0:a8daa9348a67 660 ESC_objecthandler(index, subindex);
vsluiter 0:a8daa9348a67 661 }
vsluiter 0:a8daa9348a67 662 else
vsluiter 0:a8daa9348a67 663 {
vsluiter 0:a8daa9348a67 664 SDO_abort(index, subindex, ABORT_TYPEMISMATCH);
vsluiter 0:a8daa9348a67 665 }
vsluiter 0:a8daa9348a67 666 }
vsluiter 0:a8daa9348a67 667 else
vsluiter 0:a8daa9348a67 668 {
vsluiter 0:a8daa9348a67 669 if(pgm_read_word(&(objd + nsub)->access) == ATYPE_RWpre)
vsluiter 0:a8daa9348a67 670 SDO_abort(index, subindex, ABORT_NOTINTHISSTATE);
vsluiter 0:a8daa9348a67 671 else
vsluiter 0:a8daa9348a67 672 SDO_abort(index, subindex, ABORT_READONLY);
vsluiter 0:a8daa9348a67 673 }
vsluiter 0:a8daa9348a67 674 }
vsluiter 0:a8daa9348a67 675 else
vsluiter 0:a8daa9348a67 676 {
vsluiter 0:a8daa9348a67 677 SDO_abort(index, subindex, ABORT_NOSUBINDEX);
vsluiter 0:a8daa9348a67 678 }
vsluiter 0:a8daa9348a67 679 }
vsluiter 0:a8daa9348a67 680 else
vsluiter 0:a8daa9348a67 681 {
vsluiter 0:a8daa9348a67 682 SDO_abort(index, subindex, ABORT_NOOBJECT);
vsluiter 0:a8daa9348a67 683 }
vsluiter 0:a8daa9348a67 684 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 685 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 686 }
vsluiter 0:a8daa9348a67 687
vsluiter 0:a8daa9348a67 688 void SDO_infoerror(uint32 abortcode)
vsluiter 0:a8daa9348a67 689 {
vsluiter 0:a8daa9348a67 690 uint8 MBXout;
vsluiter 0:a8daa9348a67 691 _COEobjdesc *coeres;
vsluiter 0:a8daa9348a67 692 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 693 if (MBXout)
vsluiter 0:a8daa9348a67 694 {
vsluiter 0:a8daa9348a67 695 coeres = (_COEobjdesc *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 696 coeres->mbxheader.length = htoes((uint16)0x0a);
vsluiter 0:a8daa9348a67 697 coeres->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 698 coeres->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
vsluiter 0:a8daa9348a67 699 coeres->infoheader.opcode = COE_INFOERROR; //SDO info error request
vsluiter 0:a8daa9348a67 700 coeres->infoheader.incomplete = 0;
vsluiter 0:a8daa9348a67 701 coeres->infoheader.reserved = 0x00;
vsluiter 0:a8daa9348a67 702 coeres->infoheader.fragmentsleft = 0;
vsluiter 0:a8daa9348a67 703 coeres->index = htoel(abortcode);
vsluiter 0:a8daa9348a67 704 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 705 }
vsluiter 0:a8daa9348a67 706 }
vsluiter 0:a8daa9348a67 707
vsluiter 0:a8daa9348a67 708 #define ODLISTSIZE ((MBX1_sml - MBXHSIZE - sizeof(_COEh) - sizeof(_INFOh) - 2) & 0xfffe)
vsluiter 0:a8daa9348a67 709
vsluiter 0:a8daa9348a67 710 void SDO_getodlist(void)
vsluiter 0:a8daa9348a67 711 {
vsluiter 0:a8daa9348a67 712 uint16 frags;
vsluiter 0:a8daa9348a67 713 uint8 MBXout = 0;
vsluiter 0:a8daa9348a67 714 uint16 entries = 0;
vsluiter 0:a8daa9348a67 715 uint16 i, n;
vsluiter 0:a8daa9348a67 716 uint16 *p;
vsluiter 0:a8daa9348a67 717 _COEobjdesc *coel, *coer;
vsluiter 0:a8daa9348a67 718
vsluiter 0:a8daa9348a67 719 while (pgm_read_word(&SDOobjects[entries].index) != 0xffff) entries++;
vsluiter 0:a8daa9348a67 720 ESCvar.entries = entries;
vsluiter 0:a8daa9348a67 721 frags = ((entries << 1) + ODLISTSIZE - 1);
vsluiter 0:a8daa9348a67 722 frags /= ODLISTSIZE;
vsluiter 0:a8daa9348a67 723 coer = (_COEobjdesc *)&MBX[0];
vsluiter 0:a8daa9348a67 724 if (etohs(coer->index) > 0x01) // check for unsupported opcodes
vsluiter 0:a8daa9348a67 725 {
vsluiter 0:a8daa9348a67 726 SDO_infoerror(ABORT_UNSUPPORTED);
vsluiter 0:a8daa9348a67 727 }
vsluiter 0:a8daa9348a67 728 else
vsluiter 0:a8daa9348a67 729 {
vsluiter 0:a8daa9348a67 730 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 731 }
vsluiter 0:a8daa9348a67 732 if (MBXout)
vsluiter 0:a8daa9348a67 733 {
vsluiter 0:a8daa9348a67 734 coel = (_COEobjdesc *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 735 coel->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 736 coel->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
vsluiter 0:a8daa9348a67 737 coel->infoheader.opcode = COE_GETODLISTRESPONSE;
vsluiter 0:a8daa9348a67 738 if (etohs(coer->index) == 0x00) //number of objects request
vsluiter 0:a8daa9348a67 739 {
vsluiter 0:a8daa9348a67 740 coel->index = htoes((uint16)0x00);
vsluiter 0:a8daa9348a67 741 coel->infoheader.incomplete = 0;
vsluiter 0:a8daa9348a67 742 coel->infoheader.reserved = 0x00;
vsluiter 0:a8daa9348a67 743 coel->infoheader.fragmentsleft = htoes((uint16)0);
vsluiter 0:a8daa9348a67 744 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 745 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 746 ESCvar.frags = frags;
vsluiter 0:a8daa9348a67 747 ESCvar.fragsleft = frags - 1;
vsluiter 0:a8daa9348a67 748 p = &(coel->datatype);
vsluiter 0:a8daa9348a67 749 *p = htoes(entries);
vsluiter 0:a8daa9348a67 750 p++;
vsluiter 0:a8daa9348a67 751 *p = 0;
vsluiter 0:a8daa9348a67 752 p++;
vsluiter 0:a8daa9348a67 753 *p = 0;
vsluiter 0:a8daa9348a67 754 p++;
vsluiter 0:a8daa9348a67 755 *p = 0;
vsluiter 0:a8daa9348a67 756 p++;
vsluiter 0:a8daa9348a67 757 *p = 0;
vsluiter 0:a8daa9348a67 758 coel->mbxheader.length = htoes(0x08 + (5 << 1));
vsluiter 0:a8daa9348a67 759 }
vsluiter 0:a8daa9348a67 760 if (etohs(coer->index) == 0x01) //only return all objects
vsluiter 0:a8daa9348a67 761 {
vsluiter 0:a8daa9348a67 762 if (frags > 1)
vsluiter 0:a8daa9348a67 763 {
vsluiter 0:a8daa9348a67 764 coel->infoheader.incomplete = 1;
vsluiter 0:a8daa9348a67 765 ESCvar.xoe = MBXCOE + MBXODL;
vsluiter 0:a8daa9348a67 766 n = ODLISTSIZE >> 1;
vsluiter 0:a8daa9348a67 767 }
vsluiter 0:a8daa9348a67 768 else
vsluiter 0:a8daa9348a67 769 {
vsluiter 0:a8daa9348a67 770 coel->infoheader.incomplete = 0;
vsluiter 0:a8daa9348a67 771 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 772 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 773 n = entries;
vsluiter 0:a8daa9348a67 774 }
vsluiter 0:a8daa9348a67 775 coel->infoheader.reserved = 0x00;
vsluiter 0:a8daa9348a67 776 ESCvar.frags = frags;
vsluiter 0:a8daa9348a67 777 ESCvar.fragsleft = frags - 1;
vsluiter 0:a8daa9348a67 778 coel->infoheader.fragmentsleft = htoes(ESCvar.fragsleft);
vsluiter 0:a8daa9348a67 779 coel->index = htoes((uint16)0x01);
vsluiter 0:a8daa9348a67 780
vsluiter 0:a8daa9348a67 781 p = &(coel->datatype);
vsluiter 0:a8daa9348a67 782 for (i = 0 ; i < n ; i++)
vsluiter 0:a8daa9348a67 783 {
vsluiter 0:a8daa9348a67 784 *p = htoes(pgm_read_word(&SDOobjects[i].index));
vsluiter 0:a8daa9348a67 785 p++;
vsluiter 0:a8daa9348a67 786 }
vsluiter 0:a8daa9348a67 787
vsluiter 0:a8daa9348a67 788 coel->mbxheader.length = htoes(0x08 + ( n << 1));
vsluiter 0:a8daa9348a67 789 }
vsluiter 0:a8daa9348a67 790 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 791 }
vsluiter 0:a8daa9348a67 792 }
vsluiter 0:a8daa9348a67 793
vsluiter 0:a8daa9348a67 794 void SDO_getodlistcont(void)
vsluiter 0:a8daa9348a67 795 {
vsluiter 0:a8daa9348a67 796 uint8 MBXout;
vsluiter 0:a8daa9348a67 797 uint16 i , n, s;
vsluiter 0:a8daa9348a67 798 uint16 *p;
vsluiter 0:a8daa9348a67 799 _COEobjdesc *coel;
vsluiter 0:a8daa9348a67 800
vsluiter 0:a8daa9348a67 801 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 802 if (MBXout)
vsluiter 0:a8daa9348a67 803 {
vsluiter 0:a8daa9348a67 804 coel = (_COEobjdesc *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 805 coel->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 806 coel->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
vsluiter 0:a8daa9348a67 807 coel->infoheader.opcode = COE_GETODLISTRESPONSE;
vsluiter 0:a8daa9348a67 808 s = (ESCvar.frags - ESCvar.fragsleft) * (ODLISTSIZE >> 1);
vsluiter 0:a8daa9348a67 809 if (ESCvar.fragsleft > 1)
vsluiter 0:a8daa9348a67 810 {
vsluiter 0:a8daa9348a67 811 coel->infoheader.incomplete = 1;
vsluiter 0:a8daa9348a67 812 n = s + (ODLISTSIZE >> 1);
vsluiter 0:a8daa9348a67 813 }
vsluiter 0:a8daa9348a67 814 else
vsluiter 0:a8daa9348a67 815 {
vsluiter 0:a8daa9348a67 816 coel->infoheader.incomplete = 0;
vsluiter 0:a8daa9348a67 817 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 818 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 819 n = ESCvar.entries;
vsluiter 0:a8daa9348a67 820 }
vsluiter 0:a8daa9348a67 821 coel->infoheader.reserved = 0x00;
vsluiter 0:a8daa9348a67 822 ESCvar.fragsleft--;
vsluiter 0:a8daa9348a67 823 coel->infoheader.fragmentsleft = htoes(ESCvar.fragsleft);
vsluiter 0:a8daa9348a67 824 p = &(coel->index); //pointer 2 bytes back to exclude index
vsluiter 0:a8daa9348a67 825 for ( i = s ; i < n ; i++)
vsluiter 0:a8daa9348a67 826 {
vsluiter 0:a8daa9348a67 827 *p = htoes(pgm_read_word(&SDOobjects[i].index));
vsluiter 0:a8daa9348a67 828 p++;
vsluiter 0:a8daa9348a67 829 }
vsluiter 0:a8daa9348a67 830 coel->mbxheader.length = htoes(0x06 + ((n - s) << 1));
vsluiter 0:a8daa9348a67 831 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 832 }
vsluiter 0:a8daa9348a67 833 }
vsluiter 0:a8daa9348a67 834
vsluiter 0:a8daa9348a67 835 void SDO_getod(void)
vsluiter 0:a8daa9348a67 836 {
vsluiter 0:a8daa9348a67 837 uint8 MBXout;
vsluiter 0:a8daa9348a67 838 uint16 index;
vsluiter 0:a8daa9348a67 839 int16 nidx;
vsluiter 0:a8daa9348a67 840 uint8 *d;
vsluiter 0:a8daa9348a67 841 uint8 FLASHSTORE *s;
vsluiter 0:a8daa9348a67 842 uint8 n = 0;
vsluiter 0:a8daa9348a67 843 _COEobjdesc *coer, *coel;
vsluiter 0:a8daa9348a67 844 coer = (_COEobjdesc *)&MBX[0];
vsluiter 0:a8daa9348a67 845 index = etohs(coer->index);
vsluiter 0:a8daa9348a67 846 nidx = SDO_findobject(index);
vsluiter 0:a8daa9348a67 847 if (nidx >= 0)
vsluiter 0:a8daa9348a67 848 {
vsluiter 0:a8daa9348a67 849 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 850 if (MBXout)
vsluiter 0:a8daa9348a67 851 {
vsluiter 0:a8daa9348a67 852 coel = (_COEobjdesc *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 853 coel->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 854 coel->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
vsluiter 0:a8daa9348a67 855 coel->infoheader.opcode = COE_GETODRESPONSE;
vsluiter 0:a8daa9348a67 856 coel->infoheader.incomplete = 0;
vsluiter 0:a8daa9348a67 857 coel->infoheader.reserved = 0x00;
vsluiter 0:a8daa9348a67 858 coel->infoheader.fragmentsleft = htoes(0);
vsluiter 0:a8daa9348a67 859 coel->index = htoes(index);
vsluiter 0:a8daa9348a67 860 coel->datatype = htoes(0);
vsluiter 0:a8daa9348a67 861 coel->maxsub = pgm_read_byte(&SDOobjects[nidx].maxsub);
vsluiter 0:a8daa9348a67 862 coel->objectcode = pgm_read_word(&SDOobjects[nidx].objtype);
vsluiter 0:a8daa9348a67 863 s = (uint8 *)pgm_read_word(&SDOobjects[nidx].name);
vsluiter 0:a8daa9348a67 864 d = (uint8 *)&(coel->name);
vsluiter 0:a8daa9348a67 865 while (pgm_read_byte(s) && (n < (MBXDSIZE - 0x0c)))
vsluiter 0:a8daa9348a67 866 {
vsluiter 0:a8daa9348a67 867 *d = pgm_read_byte(s);
vsluiter 0:a8daa9348a67 868 n++;
vsluiter 0:a8daa9348a67 869 s++;
vsluiter 0:a8daa9348a67 870 d++;
vsluiter 0:a8daa9348a67 871 }
vsluiter 0:a8daa9348a67 872 *d = pgm_read_byte(s);
vsluiter 0:a8daa9348a67 873 coel->mbxheader.length = htoes((uint16)0x0c + n);
vsluiter 0:a8daa9348a67 874 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 875 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 876 ESCvar.xoe=0;
vsluiter 0:a8daa9348a67 877 }
vsluiter 0:a8daa9348a67 878 }
vsluiter 0:a8daa9348a67 879 else
vsluiter 0:a8daa9348a67 880 {
vsluiter 0:a8daa9348a67 881 SDO_infoerror(ABORT_NOOBJECT);
vsluiter 0:a8daa9348a67 882 }
vsluiter 0:a8daa9348a67 883 }
vsluiter 0:a8daa9348a67 884
vsluiter 0:a8daa9348a67 885 void SDO_geted(void)
vsluiter 0:a8daa9348a67 886 {
vsluiter 0:a8daa9348a67 887 uint8 MBXout;
vsluiter 0:a8daa9348a67 888 uint16 index;
vsluiter 0:a8daa9348a67 889 int16 nidx, nsub;
vsluiter 0:a8daa9348a67 890 uint8 subindex;
vsluiter 0:a8daa9348a67 891 uint8 *d;
vsluiter 0:a8daa9348a67 892 uint8 FLASHSTORE *s;
vsluiter 0:a8daa9348a67 893 _objd FLASHSTORE *objd;
vsluiter 0:a8daa9348a67 894 uint8 n = 0;
vsluiter 0:a8daa9348a67 895 _COEentdesc *coer, *coel;
vsluiter 0:a8daa9348a67 896 coer = (_COEentdesc *)&MBX[0];
vsluiter 0:a8daa9348a67 897 index = etohs(coer->index);
vsluiter 0:a8daa9348a67 898 subindex = coer->subindex;
vsluiter 0:a8daa9348a67 899 nidx = SDO_findobject(index);
vsluiter 0:a8daa9348a67 900 if (nidx >= 0)
vsluiter 0:a8daa9348a67 901 {
vsluiter 0:a8daa9348a67 902 nsub = SDO_findsubindex(nidx, subindex);
vsluiter 0:a8daa9348a67 903 if (nsub >= 0)
vsluiter 0:a8daa9348a67 904 {
vsluiter 0:a8daa9348a67 905 objd = (_objd FLASHSTORE *)pgm_read_word(&SDOobjects[nidx].objdesc);
vsluiter 0:a8daa9348a67 906 MBXout = ESC_claimbuffer();
vsluiter 0:a8daa9348a67 907 if (MBXout)
vsluiter 0:a8daa9348a67 908 {
vsluiter 0:a8daa9348a67 909 coel = (_COEentdesc *)&MBX[MBXout];
vsluiter 0:a8daa9348a67 910 coel->mbxheader.mbxtype = MBXCOE;
vsluiter 0:a8daa9348a67 911 coel->coeheader.numberservice = htoes((0 & 0x01f) | (COE_SDOINFORMATION << 12));
vsluiter 0:a8daa9348a67 912 coel->infoheader.opcode = COE_ENTRYDESCRIPTIONRESPONSE;
vsluiter 0:a8daa9348a67 913 coel->infoheader.incomplete = 0;
vsluiter 0:a8daa9348a67 914 coel->infoheader.reserved = 0x00;
vsluiter 0:a8daa9348a67 915 coel->infoheader.fragmentsleft = htoes((uint16)0);
vsluiter 0:a8daa9348a67 916 coel->index = htoes(index);
vsluiter 0:a8daa9348a67 917 coel->subindex = subindex;
vsluiter 0:a8daa9348a67 918 coel->valueinfo = COE_VALUEINFO_ACCESS +
vsluiter 0:a8daa9348a67 919 COE_VALUEINFO_OBJECT +
vsluiter 0:a8daa9348a67 920 COE_VALUEINFO_MAPPABLE;
vsluiter 0:a8daa9348a67 921 coel->datatype = htoes(pgm_read_word(&(objd + nsub)->datatype));
vsluiter 0:a8daa9348a67 922 coel->bitlength = htoes(pgm_read_word(&(objd + nsub)->bitlength));
vsluiter 0:a8daa9348a67 923 coel->access = htoes(pgm_read_word(&(objd + nsub)->access));
vsluiter 0:a8daa9348a67 924 s = (uint8 *)pgm_read_word(&(objd+nsub)->name);
vsluiter 0:a8daa9348a67 925 d = (uint8 *)&(coel->name);
vsluiter 0:a8daa9348a67 926 while (pgm_read_byte(s) && (n < (MBXDSIZE - 0x10)))
vsluiter 0:a8daa9348a67 927 {
vsluiter 0:a8daa9348a67 928 *d = pgm_read_byte(s);
vsluiter 0:a8daa9348a67 929 n++;
vsluiter 0:a8daa9348a67 930 s++;
vsluiter 0:a8daa9348a67 931 d++;
vsluiter 0:a8daa9348a67 932 }
vsluiter 0:a8daa9348a67 933 *d = pgm_read_byte(s);
vsluiter 0:a8daa9348a67 934 coel->mbxheader.length = htoes((uint16)0x10 + n);
vsluiter 0:a8daa9348a67 935 MBXcontrol[MBXout].state = MBXstate_outreq;
vsluiter 0:a8daa9348a67 936 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 937 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 938 }
vsluiter 0:a8daa9348a67 939 }
vsluiter 0:a8daa9348a67 940 else
vsluiter 0:a8daa9348a67 941 {
vsluiter 0:a8daa9348a67 942 SDO_infoerror(ABORT_NOSUBINDEX);
vsluiter 0:a8daa9348a67 943 }
vsluiter 0:a8daa9348a67 944 }
vsluiter 0:a8daa9348a67 945 else
vsluiter 0:a8daa9348a67 946 {
vsluiter 0:a8daa9348a67 947 SDO_infoerror(ABORT_NOOBJECT);
vsluiter 0:a8daa9348a67 948 }
vsluiter 0:a8daa9348a67 949 }
vsluiter 0:a8daa9348a67 950
vsluiter 0:a8daa9348a67 951 void ESC_coeprocess(void)
vsluiter 0:a8daa9348a67 952 {
vsluiter 0:a8daa9348a67 953 _MBXh *mbh;
vsluiter 0:a8daa9348a67 954 _COEsdo *coesdo;
vsluiter 0:a8daa9348a67 955 _COEobjdesc *coeobjdesc;
vsluiter 0:a8daa9348a67 956 uint8 service;
vsluiter 0:a8daa9348a67 957 if (!MBXrun)
vsluiter 0:a8daa9348a67 958 {
vsluiter 0:a8daa9348a67 959 return;
vsluiter 0:a8daa9348a67 960 }
vsluiter 0:a8daa9348a67 961 if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
vsluiter 0:a8daa9348a67 962 {
vsluiter 0:a8daa9348a67 963 mbh = (_MBXh *)&MBX[0];
vsluiter 0:a8daa9348a67 964 if (mbh->mbxtype == MBXCOE)
vsluiter 0:a8daa9348a67 965 {
vsluiter 0:a8daa9348a67 966 ESCvar.xoe = MBXCOE;
vsluiter 0:a8daa9348a67 967 }
vsluiter 0:a8daa9348a67 968 }
vsluiter 0:a8daa9348a67 969 if ((ESCvar.xoe == (MBXCOE + MBXODL)) && (!ESCvar.mbxoutpost))
vsluiter 0:a8daa9348a67 970 {
vsluiter 0:a8daa9348a67 971 SDO_getodlistcont(); // continue get OD list
vsluiter 0:a8daa9348a67 972 }
vsluiter 0:a8daa9348a67 973 if (ESCvar.xoe == MBXCOE)
vsluiter 0:a8daa9348a67 974 {
vsluiter 0:a8daa9348a67 975 coesdo = (_COEsdo *)&MBX[0];
vsluiter 0:a8daa9348a67 976 coeobjdesc = (_COEobjdesc *)&MBX[0];
vsluiter 0:a8daa9348a67 977 service = etohs(coesdo->coeheader.numberservice) >> 12;
vsluiter 0:a8daa9348a67 978 //initiate SDO upload request
vsluiter 0:a8daa9348a67 979 if ((service == COE_SDOREQUEST)
vsluiter 0:a8daa9348a67 980 && (coesdo->command == COE_COMMAND_UPLOADREQUEST)
vsluiter 0:a8daa9348a67 981 && (etohs(coesdo->mbxheader.length) == 0x0a))
vsluiter 0:a8daa9348a67 982 {
vsluiter 0:a8daa9348a67 983 SDO_upload();
vsluiter 0:a8daa9348a67 984 }
vsluiter 0:a8daa9348a67 985 //SDO upload segment request
vsluiter 0:a8daa9348a67 986 if ((service == COE_SDOREQUEST)
vsluiter 0:a8daa9348a67 987 && ((coesdo->command & 0xef) == COE_COMMAND_UPLOADSEGREQ)
vsluiter 0:a8daa9348a67 988 && (etohs(coesdo->mbxheader.length) == 0x0a)
vsluiter 0:a8daa9348a67 989 && (ESCvar.segmented == MBXSEU))
vsluiter 0:a8daa9348a67 990 {
vsluiter 0:a8daa9348a67 991 SDO_uploadsegment();
vsluiter 0:a8daa9348a67 992 }
vsluiter 0:a8daa9348a67 993 //initiate SDO download request
vsluiter 0:a8daa9348a67 994 else
vsluiter 0:a8daa9348a67 995 if ((service == COE_SDOREQUEST)
vsluiter 0:a8daa9348a67 996 && ((coesdo->command & 0xf1) == 0x21))
vsluiter 0:a8daa9348a67 997 {
vsluiter 0:a8daa9348a67 998 SDO_download();
vsluiter 0:a8daa9348a67 999 }
vsluiter 0:a8daa9348a67 1000 //initiate SDO get OD list
vsluiter 0:a8daa9348a67 1001 else
vsluiter 0:a8daa9348a67 1002 if ((service == COE_SDOINFORMATION)
vsluiter 0:a8daa9348a67 1003 &&(coeobjdesc->infoheader.opcode == 0x01))
vsluiter 0:a8daa9348a67 1004 {
vsluiter 0:a8daa9348a67 1005 SDO_getodlist();
vsluiter 0:a8daa9348a67 1006 }
vsluiter 0:a8daa9348a67 1007 //initiate SDO get OD
vsluiter 0:a8daa9348a67 1008 else
vsluiter 0:a8daa9348a67 1009 if ((service == COE_SDOINFORMATION)
vsluiter 0:a8daa9348a67 1010 && (coeobjdesc->infoheader.opcode == 0x03))
vsluiter 0:a8daa9348a67 1011 {
vsluiter 0:a8daa9348a67 1012 SDO_getod();
vsluiter 0:a8daa9348a67 1013 }
vsluiter 0:a8daa9348a67 1014 //initiate SDO get ED
vsluiter 0:a8daa9348a67 1015 else
vsluiter 0:a8daa9348a67 1016 if ((service == COE_SDOINFORMATION)
vsluiter 0:a8daa9348a67 1017 && (coeobjdesc->infoheader.opcode == 0x05))
vsluiter 0:a8daa9348a67 1018 {
vsluiter 0:a8daa9348a67 1019 SDO_geted();
vsluiter 0:a8daa9348a67 1020 }
vsluiter 0:a8daa9348a67 1021 else
vsluiter 0:a8daa9348a67 1022 if (ESCvar.xoe == MBXCOE) //COE not recognised above
vsluiter 0:a8daa9348a67 1023 {
vsluiter 0:a8daa9348a67 1024 if (service == 0)
vsluiter 0:a8daa9348a67 1025 {
vsluiter 0:a8daa9348a67 1026 MBX_error(MBXERR_INVALIDHEADER);
vsluiter 0:a8daa9348a67 1027 }
vsluiter 0:a8daa9348a67 1028 else
vsluiter 0:a8daa9348a67 1029 {
vsluiter 0:a8daa9348a67 1030 SDO_abort(etohs(coesdo->index), coesdo->subindex, ABORT_UNKNOWN);
vsluiter 0:a8daa9348a67 1031 }
vsluiter 0:a8daa9348a67 1032 MBXcontrol[0].state = MBXstate_idle;
vsluiter 0:a8daa9348a67 1033 ESCvar.xoe = 0;
vsluiter 0:a8daa9348a67 1034 }
vsluiter 0:a8daa9348a67 1035 }
vsluiter 0:a8daa9348a67 1036 }
vsluiter 0:a8daa9348a67 1037
vsluiter 0:a8daa9348a67 1038 void ESC_xoeprocess(void)
vsluiter 0:a8daa9348a67 1039 {
vsluiter 0:a8daa9348a67 1040 _MBXh *mbh;
vsluiter 0:a8daa9348a67 1041 if (!MBXrun)
vsluiter 0:a8daa9348a67 1042 {
vsluiter 0:a8daa9348a67 1043 return;
vsluiter 0:a8daa9348a67 1044 }
vsluiter 0:a8daa9348a67 1045 if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
vsluiter 0:a8daa9348a67 1046 {
vsluiter 0:a8daa9348a67 1047 mbh = (_MBXh *)&MBX[0];
vsluiter 0:a8daa9348a67 1048 if ((mbh->mbxtype == 0) || (etohs(mbh->length) == 0))
vsluiter 0:a8daa9348a67 1049 {
vsluiter 0:a8daa9348a67 1050 MBX_error(MBXERR_INVALIDHEADER);
vsluiter 0:a8daa9348a67 1051 }
vsluiter 0:a8daa9348a67 1052 else
vsluiter 0:a8daa9348a67 1053 {
vsluiter 0:a8daa9348a67 1054 MBX_error(MBXERR_UNSUPPORTEDPROTOCOL);
vsluiter 0:a8daa9348a67 1055 }
vsluiter 0:a8daa9348a67 1056 MBXcontrol[0].state = MBXstate_idle; // mailbox type not supported, drop mailbox
vsluiter 0:a8daa9348a67 1057 }
vsluiter 0:a8daa9348a67 1058 }
vsluiter 0:a8daa9348a67 1059
vsluiter 0:a8daa9348a67 1060 uint8 ESC_checkSM23(uint8 state)
vsluiter 0:a8daa9348a67 1061 {
vsluiter 0:a8daa9348a67 1062 _ESCsm2 *SM;
vsluiter 0:a8daa9348a67 1063 ESC_read(ESCREG_SM2, &ESCvar.SM[2], sizeof(ESCvar.SM[2]), &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 1064 ESC_read(ESCREG_SM3, &ESCvar.SM[3], sizeof(ESCvar.SM[3]), &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 1065 SM = (_ESCsm2 *)&ESCvar.SM[2];
vsluiter 0:a8daa9348a67 1066 if ((etohs(SM->PSA) != SM2_sma) || (etohs(SM->Length) != SM2_sml) || (SM->Command != SM2_smc) || !(SM->ActESC & SM2_act))
vsluiter 0:a8daa9348a67 1067 {
vsluiter 0:a8daa9348a67 1068 ESCvar.SMtestresult = SMRESULT_ERRSM2;
vsluiter 0:a8daa9348a67 1069 return ESCpreop | ESCerror; //fail state change
vsluiter 0:a8daa9348a67 1070 }
vsluiter 0:a8daa9348a67 1071 SM = (_ESCsm2 *)&ESCvar.SM[3];
vsluiter 0:a8daa9348a67 1072 if ((etohs(SM->PSA) != SM3_sma) || (etohs(SM->Length) != SM3_sml) || (SM->Command != SM3_smc) || !(SM->ActESC & SM3_act))
vsluiter 0:a8daa9348a67 1073 {
vsluiter 0:a8daa9348a67 1074 ESCvar.SMtestresult = SMRESULT_ERRSM3;
vsluiter 0:a8daa9348a67 1075 return ESCpreop | ESCerror; //fail state change
vsluiter 0:a8daa9348a67 1076 }
vsluiter 0:a8daa9348a67 1077 return state;
vsluiter 0:a8daa9348a67 1078 }
vsluiter 0:a8daa9348a67 1079
vsluiter 0:a8daa9348a67 1080 uint8 ESC_startinput(uint8 state)
vsluiter 0:a8daa9348a67 1081 {
vsluiter 0:a8daa9348a67 1082 state = ESC_checkSM23(state);
vsluiter 0:a8daa9348a67 1083 if (state != (ESCpreop | ESCerror))
vsluiter 0:a8daa9348a67 1084 {
vsluiter 0:a8daa9348a67 1085 ESC_SMenable(3);
vsluiter 0:a8daa9348a67 1086 APPstate = APPSTATE_INPUT;
vsluiter 0:a8daa9348a67 1087 }
vsluiter 0:a8daa9348a67 1088 else
vsluiter 0:a8daa9348a67 1089 {
vsluiter 0:a8daa9348a67 1090 ESC_SMdisable(2);
vsluiter 0:a8daa9348a67 1091 ESC_SMdisable(3);
vsluiter 0:a8daa9348a67 1092 if( ESCvar.SMtestresult & SMRESULT_ERRSM3 )
vsluiter 0:a8daa9348a67 1093 ESC_ALerror(ALERR_INVALIDINPUTSM);
vsluiter 0:a8daa9348a67 1094 else
vsluiter 0:a8daa9348a67 1095 ESC_ALerror(ALERR_INVALIDOUTPUTSM);
vsluiter 0:a8daa9348a67 1096 }
vsluiter 0:a8daa9348a67 1097 return state;
vsluiter 0:a8daa9348a67 1098 }
vsluiter 0:a8daa9348a67 1099
vsluiter 0:a8daa9348a67 1100 void ESC_stopinput(void)
vsluiter 0:a8daa9348a67 1101 {
vsluiter 0:a8daa9348a67 1102 APPstate = APPSTATE_IDLE;
vsluiter 0:a8daa9348a67 1103 ESC_SMdisable(3);
vsluiter 0:a8daa9348a67 1104 ESC_SMdisable(2);
vsluiter 0:a8daa9348a67 1105 }
vsluiter 0:a8daa9348a67 1106
vsluiter 0:a8daa9348a67 1107 uint8 ESC_startoutput(uint8 state)
vsluiter 0:a8daa9348a67 1108 {
vsluiter 0:a8daa9348a67 1109 ESC_SMenable(2);
vsluiter 0:a8daa9348a67 1110 APPstate |= APPSTATE_OUTPUT;
vsluiter 0:a8daa9348a67 1111 return state;
vsluiter 0:a8daa9348a67 1112 }
vsluiter 0:a8daa9348a67 1113
vsluiter 0:a8daa9348a67 1114 void ESC_stopoutput(void)
vsluiter 0:a8daa9348a67 1115 {
vsluiter 0:a8daa9348a67 1116 APPstate &= APPSTATE_INPUT;
vsluiter 0:a8daa9348a67 1117 ESC_SMdisable(2);
vsluiter 0:a8daa9348a67 1118 APP_safeoutput();
vsluiter 0:a8daa9348a67 1119 }
vsluiter 0:a8daa9348a67 1120
vsluiter 0:a8daa9348a67 1121 void ESC_ALevent(void)
vsluiter 0:a8daa9348a67 1122 {
vsluiter 0:a8daa9348a67 1123 if (!(ESCvar.ALevent & (ESCREG_ALEVENT_CONTROL | ESCREG_ALEVENT_SMCHANGE)) &&
vsluiter 0:a8daa9348a67 1124 (ESCvar.wdcnt++ < 1000))
vsluiter 0:a8daa9348a67 1125 { //nothing to do
vsluiter 0:a8daa9348a67 1126 ESCvar.ALcontrol = 0;
vsluiter 0:a8daa9348a67 1127 return;
vsluiter 0:a8daa9348a67 1128 }
vsluiter 0:a8daa9348a67 1129 ESCvar.wdcnt = 0;
vsluiter 0:a8daa9348a67 1130 ESC_read(ESCREG_ALCONTROL, &ESCvar.ALcontrol, sizeof(ESCvar.ALcontrol), &ESCvar.ALevent);
vsluiter 0:a8daa9348a67 1131 ESCvar.ALcontrol = etohs(ESCvar.ALcontrol);
vsluiter 0:a8daa9348a67 1132 }
vsluiter 0:a8daa9348a67 1133
vsluiter 0:a8daa9348a67 1134 void ESC_state(void)
vsluiter 0:a8daa9348a67 1135 {
vsluiter 0:a8daa9348a67 1136 uint8 ac, an, as, ax;
vsluiter 0:a8daa9348a67 1137 if (!ESCvar.ALcontrol) return; //nothing to do
vsluiter 0:a8daa9348a67 1138 ac = ESCvar.ALcontrol & 0x001f;
vsluiter 0:a8daa9348a67 1139 as = ESCvar.ALstatus & 0x001f;
vsluiter 0:a8daa9348a67 1140 an = as;
vsluiter 0:a8daa9348a67 1141 if (((ac & ESCerror) || (ac == ESCinit)))
vsluiter 0:a8daa9348a67 1142 {
vsluiter 0:a8daa9348a67 1143 ac &= 0x0f; // if error bit confirmed reset
vsluiter 0:a8daa9348a67 1144 an &= 0x0f;
vsluiter 0:a8daa9348a67 1145 }
vsluiter 0:a8daa9348a67 1146 if ((ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE) &&
vsluiter 0:a8daa9348a67 1147 (as & 0x0e) &&
vsluiter 0:a8daa9348a67 1148 !(ESCvar.ALevent & ESCREG_ALEVENT_CONTROL) &&
vsluiter 0:a8daa9348a67 1149 MBXrun)
vsluiter 0:a8daa9348a67 1150 {
vsluiter 0:a8daa9348a67 1151 ESCvar.ALevent = 0;
vsluiter 0:a8daa9348a67 1152 ax = ESC_checkmbx(as);
vsluiter 0:a8daa9348a67 1153 if ((as & ESCerror) && ((ac != (ESCinit | ESCerror)) || (ac != ESCinit)))
vsluiter 0:a8daa9348a67 1154 return; // if in error then stay there
vsluiter 0:a8daa9348a67 1155 if (ax == (ESCinit | ESCerror))
vsluiter 0:a8daa9348a67 1156 {
vsluiter 0:a8daa9348a67 1157 if (APPstate)
vsluiter 0:a8daa9348a67 1158 {
vsluiter 0:a8daa9348a67 1159 ESC_stopoutput();
vsluiter 0:a8daa9348a67 1160 ESC_stopinput();
vsluiter 0:a8daa9348a67 1161 }
vsluiter 0:a8daa9348a67 1162 ESC_stopmbx();
vsluiter 0:a8daa9348a67 1163 ESC_ALerror(ALERR_INVALIDMBXCONFIG);
vsluiter 0:a8daa9348a67 1164 MBXrun = 0;
vsluiter 0:a8daa9348a67 1165 ESC_ALstatus(ax);
vsluiter 0:a8daa9348a67 1166 return;
vsluiter 0:a8daa9348a67 1167 }
vsluiter 0:a8daa9348a67 1168 ax = ESC_checkSM23(as);
vsluiter 0:a8daa9348a67 1169 if ((APPstate) && (ax == (ESCpreop | ESCerror)))
vsluiter 0:a8daa9348a67 1170 {
vsluiter 0:a8daa9348a67 1171 ESC_stopoutput();
vsluiter 0:a8daa9348a67 1172 ESC_stopinput();
vsluiter 0:a8daa9348a67 1173 if( ESCvar.SMtestresult & SMRESULT_ERRSM3 )
vsluiter 0:a8daa9348a67 1174 ESC_ALerror(ALERR_INVALIDINPUTSM);
vsluiter 0:a8daa9348a67 1175 else
vsluiter 0:a8daa9348a67 1176 ESC_ALerror(ALERR_INVALIDOUTPUTSM);
vsluiter 0:a8daa9348a67 1177 ESC_ALstatus(ax);
vsluiter 0:a8daa9348a67 1178 return;
vsluiter 0:a8daa9348a67 1179 }
vsluiter 0:a8daa9348a67 1180 }
vsluiter 0:a8daa9348a67 1181 ESCvar.ALevent = 0;
vsluiter 0:a8daa9348a67 1182 if ((an & ESCerror) && !(ac & ESCerror)) return; //error state not acked, leave original
vsluiter 0:a8daa9348a67 1183 as = (ac << 4) | (as & 0x0f); // high bits ALcommand, low bits ALstatus
vsluiter 0:a8daa9348a67 1184 switch (as)
vsluiter 0:a8daa9348a67 1185 {
vsluiter 0:a8daa9348a67 1186 case INIT_TO_INIT:
vsluiter 0:a8daa9348a67 1187 case PREOP_TO_PREOP:
vsluiter 0:a8daa9348a67 1188 case OP_TO_OP:
vsluiter 0:a8daa9348a67 1189 break;
vsluiter 0:a8daa9348a67 1190 case INIT_TO_PREOP:
vsluiter 0:a8daa9348a67 1191 ESC_address(); // get station address
vsluiter 0:a8daa9348a67 1192 an = ESC_startmbx(ac);
vsluiter 0:a8daa9348a67 1193 break;
vsluiter 0:a8daa9348a67 1194 case INIT_TO_BOOT:
vsluiter 0:a8daa9348a67 1195 an = ESCinit | ESCerror;
vsluiter 0:a8daa9348a67 1196 ESC_ALerror(ALERR_BOOTNOTSUPPORTED);
vsluiter 0:a8daa9348a67 1197 break;
vsluiter 0:a8daa9348a67 1198 case INIT_TO_SAFEOP:
vsluiter 0:a8daa9348a67 1199 case INIT_TO_OP:
vsluiter 0:a8daa9348a67 1200 an = ESCinit | ESCerror;
vsluiter 0:a8daa9348a67 1201 ESC_ALerror(ALERR_INVALIDSTATECHANGE);
vsluiter 0:a8daa9348a67 1202 break;
vsluiter 0:a8daa9348a67 1203 case OP_TO_INIT:
vsluiter 0:a8daa9348a67 1204 ESC_stopoutput();
vsluiter 0:a8daa9348a67 1205 case SAFEOP_TO_INIT:
vsluiter 0:a8daa9348a67 1206 ESC_stopinput();
vsluiter 0:a8daa9348a67 1207 case PREOP_TO_INIT:
vsluiter 0:a8daa9348a67 1208 ESC_stopmbx();
vsluiter 0:a8daa9348a67 1209 an = ESCinit;
vsluiter 0:a8daa9348a67 1210 break;
vsluiter 0:a8daa9348a67 1211 case PREOP_TO_BOOT:
vsluiter 0:a8daa9348a67 1212 an = ESCpreop | ESCerror;
vsluiter 0:a8daa9348a67 1213 ESC_ALerror(ALERR_INVALIDSTATECHANGE);
vsluiter 0:a8daa9348a67 1214 break;
vsluiter 0:a8daa9348a67 1215 case PREOP_TO_SAFEOP:
vsluiter 0:a8daa9348a67 1216 case SAFEOP_TO_SAFEOP:
vsluiter 0:a8daa9348a67 1217 SM2_sml = sizeRXPDO();
vsluiter 0:a8daa9348a67 1218 SM3_sml = sizeTXPDO();
vsluiter 0:a8daa9348a67 1219 an = ESC_startinput(ac);
vsluiter 0:a8daa9348a67 1220 if (an == ac) ESC_SMenable(2);
vsluiter 0:a8daa9348a67 1221 break;
vsluiter 0:a8daa9348a67 1222 case PREOP_TO_OP:
vsluiter 0:a8daa9348a67 1223 an = ESCpreop | ESCerror;
vsluiter 0:a8daa9348a67 1224 ESC_ALerror(ALERR_INVALIDSTATECHANGE);
vsluiter 0:a8daa9348a67 1225 break;
vsluiter 0:a8daa9348a67 1226 case OP_TO_PREOP:
vsluiter 0:a8daa9348a67 1227 ESC_stopoutput();
vsluiter 0:a8daa9348a67 1228 case SAFEOP_TO_PREOP:
vsluiter 0:a8daa9348a67 1229 ESC_stopinput();
vsluiter 0:a8daa9348a67 1230 an = ESCpreop;
vsluiter 0:a8daa9348a67 1231 break;
vsluiter 0:a8daa9348a67 1232 case SAFEOP_TO_BOOT:
vsluiter 0:a8daa9348a67 1233 an = ESCsafeop | ESCerror;
vsluiter 0:a8daa9348a67 1234 ESC_ALerror(ALERR_INVALIDSTATECHANGE);
vsluiter 0:a8daa9348a67 1235 break;
vsluiter 0:a8daa9348a67 1236 case SAFEOP_TO_OP:
vsluiter 0:a8daa9348a67 1237 an = ESC_startoutput(ac);
vsluiter 0:a8daa9348a67 1238 break;
vsluiter 0:a8daa9348a67 1239 case OP_TO_BOOT:
vsluiter 0:a8daa9348a67 1240 an = ESCsafeop | ESCerror;
vsluiter 0:a8daa9348a67 1241 ESC_ALerror(ALERR_INVALIDSTATECHANGE);
vsluiter 0:a8daa9348a67 1242 ESC_stopoutput();
vsluiter 0:a8daa9348a67 1243 break;
vsluiter 0:a8daa9348a67 1244 case OP_TO_SAFEOP:
vsluiter 0:a8daa9348a67 1245 an = ESCsafeop;
vsluiter 0:a8daa9348a67 1246 ESC_stopoutput();
vsluiter 0:a8daa9348a67 1247 break;
vsluiter 0:a8daa9348a67 1248 default :
vsluiter 0:a8daa9348a67 1249 if (an == ESCop)
vsluiter 0:a8daa9348a67 1250 {
vsluiter 0:a8daa9348a67 1251 ESC_stopoutput();
vsluiter 0:a8daa9348a67 1252 an = ESCsafeop;
vsluiter 0:a8daa9348a67 1253 }
vsluiter 0:a8daa9348a67 1254 if (as == ESCsafeop)
vsluiter 0:a8daa9348a67 1255 ESC_stopinput();
vsluiter 0:a8daa9348a67 1256 an |= ESCerror;
vsluiter 0:a8daa9348a67 1257 ESC_ALerror(ALERR_UNKNOWNSTATE);
vsluiter 0:a8daa9348a67 1258 }
vsluiter 0:a8daa9348a67 1259 if (!(an & ESCerror) && (ESCvar.ALerror))
vsluiter 0:a8daa9348a67 1260 {
vsluiter 0:a8daa9348a67 1261 ESC_ALerror(ALERR_NONE); // clear error
vsluiter 0:a8daa9348a67 1262 }
vsluiter 0:a8daa9348a67 1263 ESC_ALstatus(an);
vsluiter 0:a8daa9348a67 1264 }
vsluiter 0:a8daa9348a67 1265