Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Tammo Jan Dijkema
dt_ctrl
Commits
1ebd504b
Commit
1ebd504b
authored
Feb 19, 2013
by
Jeroen Vreeken
Browse files
canopen emergency support
parent
b093ebb6
Changes
7
Hide whitespace changes
Inline
Side-by-side
controller/ec/block_stoeber.c
View file @
1ebd504b
...
...
@@ -251,6 +251,8 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
stoeber_rx
->
private
=
private
;
stoeber_tx
->
private
=
private
;
memset
(
&
private
->
stbr
,
0
,
sizeof
(
private
->
stbr
));
ec_addr_set_auto_inc_nr
(
&
private
->
stbr
.
addr
,
ec_pos
);
private
->
stbr
.
max_speed
=
RPM2RADS
(
3000
);
...
...
@@ -263,6 +265,8 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
if
(
ec_stoeber_init
(
&
private
->
stbr
))
goto
err_stoeber_init
;
private
->
stbr
.
canopen_dev
->
logname
=
stoeber
->
name
;
printf
(
"Drive serial number: %d
\n
"
,
private
->
stbr
.
serial_number
);
if
(
serial_number
&&
serial_number
!=
private
->
stbr
.
serial_number
)
{
printf
(
"Serial number of drive (%d) does not match requested number (%d)
\n
"
,
...
...
controller/ec/canopen.c
View file @
1ebd504b
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007
Copyright Stichting C.A. Muller Radioastronomiestation, 2007
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007
, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2007
, 2013
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
...
...
@@ -19,13 +19,16 @@
#include
<string.h>
#include
<stdio.h>
#include
<pthread.h>
#include
"canopen.h"
#include
"log.h"
#define CANOPEN_CSS_MASK 0xe0
#define CANOPEN_CSS_INITIATE_DOMAIN_DOWNLOAD (0x1 << 5)
#define CANOPEN_CSS_INITIATE_DOMAIN_UPLOAD (0x2 << 5)
#define CANOPEN_CSS_ABORT (0x4 << 5)
#define CANOPEN_CSS_INITIATE_DOMAIN_DOWNLOAD (0x1 << 5)
// 0x20
#define CANOPEN_CSS_INITIATE_DOMAIN_UPLOAD (0x2 << 5)
// 0x40
#define CANOPEN_CSS_ABORT (0x4 << 5)
// 0x80
#define CANOPEN_N_MASK 0x0c
#define CANOPEN_N_SHIFT 2
#define CANOPEN_E 0x02
...
...
@@ -47,10 +50,14 @@ ssize_t canopen_write_param(struct canopen_dev *dev,
cmd
[
2
]
=
index
>>
8
;
cmd
[
3
]
=
subindex
&
0xff
;
pthread_mutex_lock
(
&
dev
->
lock
);
ret
=
dev
->
send_request
(
dev
,
cmd
,
8
);
if
(
ret
!=
8
)
return
-
1
;
ret
=
dev
->
recv_response
(
dev
,
cmd
,
8
);
if
(
ret
==
8
)
ret
=
dev
->
recv_response
(
dev
,
cmd
,
8
);
pthread_mutex_unlock
(
&
dev
->
lock
);
if
(
ret
!=
8
)
return
-
1
;
...
...
@@ -74,10 +81,15 @@ ssize_t canopen_read_param(struct canopen_dev *dev,
cmd
[
2
]
=
index
>>
8
;
cmd
[
3
]
=
subindex
&
0xff
;
pthread_mutex_lock
(
&
dev
->
lock
);
ret
=
dev
->
send_request
(
dev
,
cmd
,
8
);
if
(
ret
!=
8
)
return
-
1
;
ret
=
dev
->
recv_response
(
dev
,
cmd
,
8
);
if
(
ret
==
8
)
ret
=
dev
->
recv_response
(
dev
,
cmd
,
8
);
pthread_mutex_unlock
(
&
dev
->
lock
);
if
(
ret
!=
8
)
return
-
1
;
...
...
@@ -96,3 +108,91 @@ ssize_t canopen_read_param(struct canopen_dev *dev,
return
ret
;
}
void
canopen_emergency_handler
(
struct
canopen_dev
*
dev
,
void
*
msg
,
size_t
len
)
{
uint16_t
code_le
,
code
;
uint8_t
reg
;
uint8_t
vendor
[
5
];
if
(
len
!=
8
)
{
log_send
(
LOG_T_ERROR
,
"CANopen emergency received with wrong length: %zd"
,
len
);
}
else
{
int
log_type
=
LOG_T_ERROR
;
char
*
category
=
"unknown"
;
char
*
cause
=
"unknown"
;
memcpy
(
&
code_le
,
msg
,
2
);
code
=
le16toh
(
code_le
);
memcpy
(
&
reg
,
msg
+
2
,
1
);
memcpy
(
vendor
,
msg
+
3
,
5
);
if
(
reg
&
0x01
)
cause
=
"Generic Error"
;
else
if
(
reg
&
0x02
)
cause
=
"Current"
;
else
if
(
reg
&
0x04
)
cause
=
"Voltage"
;
else
if
(
reg
&
0x08
)
cause
=
"Temperature"
;
else
if
(
reg
&
0x10
)
cause
=
"Communication Error"
;
else
if
(
reg
&
0x20
)
cause
=
"Device Profile Specific"
;
else
if
(
reg
&
0x40
)
cause
=
"Reserved"
;
else
if
(
reg
&
0x80
)
cause
=
"Manufacturer Specific"
;
if
(
!
(
code
&
0xff00
))
{
category
=
"Error reset"
;
log_type
=
LOG_T_WARNING
;
}
else
if
((
code
&
0xff00
)
==
0x1000
)
{
category
=
"Generic Error"
;
}
else
if
((
code
&
0xf000
)
==
0x2000
)
{
category
=
"Current"
;
}
else
if
((
code
&
0xf000
)
==
0x3000
)
{
category
=
"Voltage"
;
}
else
if
((
code
&
0xf000
)
==
0x4000
)
{
category
=
"Temperature"
;
}
else
if
((
code
&
0xff00
)
==
0x5000
)
{
category
=
"Device Hardware"
;
}
else
if
((
code
&
0xff00
)
==
0x6000
)
{
category
=
"Device Software"
;
}
else
if
((
code
&
0xff00
)
==
0x7000
)
{
category
=
"Additional Modules"
;
}
else
if
((
code
&
0xff00
)
==
0x8000
)
{
category
=
"Monitoring"
;
}
else
if
((
code
&
0xff00
)
==
0x9000
)
{
category
=
"External Error"
;
}
else
if
((
code
&
0xff00
)
==
0xf000
)
{
category
=
"Additional Functions"
;
}
else
if
((
code
&
0xff00
)
==
0xff00
)
{
category
=
"Device Specific"
;
}
log_send
(
log_type
,
"%s: CANopen emergency message: "
"errorcode=0x%04x (%s), register=0x%02x (%s), "
"vendor=0x%02x%02x%02x%02x%02x"
,
dev
->
logname
,
code
,
category
,
reg
,
cause
,
vendor
[
0
],
vendor
[
1
],
vendor
[
2
],
vendor
[
3
],
vendor
[
4
]);
if
(
dev
->
emergency_handler
)
dev
->
emergency_handler
(
dev
,
code
,
reg
,
vendor
);
}
}
void
canopen_emergency_handler_attach
(
struct
canopen_dev
*
dev
,
void
(
*
func
)(
struct
canopen_dev
*
dev
,
uint16_t
code
,
uint8_t
register
,
uint8_t
vendor
[
5
]))
{
dev
->
emergency_handler
=
func
;
}
int
canopen_init
(
struct
canopen_dev
*
dev
)
{
return
pthread_mutex_init
(
&
dev
->
lock
,
NULL
);
}
controller/ec/canopen.h
View file @
1ebd504b
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007
Copyright Stichting C.A. Muller Radioastronomiestation, 2007
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007
, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2007
, 2013
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
...
...
@@ -21,10 +21,17 @@
#define _INCLUDE_CANOPEN_H_
#include
<unistd.h>
#include
<stdint.h>
struct
canopen_dev
{
ssize_t
(
*
send_request
)(
struct
canopen_dev
*
dev
,
void
*
req
,
size_t
len
);
ssize_t
(
*
recv_response
)(
struct
canopen_dev
*
dev
,
void
*
res
,
size_t
len
);
void
(
*
emergency_handler
)(
struct
canopen_dev
*
dev
,
uint16_t
code
,
uint8_t
reg
,
uint8_t
vendor
[
5
]);
char
*
logname
;
pthread_mutex_t
lock
;
struct
canopen_dev_priv
*
priv
;
};
...
...
@@ -35,4 +42,11 @@ ssize_t canopen_read_param(struct canopen_dev *dev,
unsigned
index
,
unsigned
subindex
,
void
*
data
,
ssize_t
bytes
);
void
canopen_emergency_handler_attach
(
struct
canopen_dev
*
dev
,
void
(
*
func
)(
struct
canopen_dev
*
dev
,
uint16_t
code
,
uint8_t
register
,
uint8_t
vendor
[
5
]));
int
canopen_init
(
struct
canopen_dev
*
dev
);
void
canopen_emergency_handler
(
struct
canopen_dev
*
dev
,
void
*
msg
,
size_t
len
);
#endif
/* _INCLUDE_CANOPEN_H_ */
controller/ec/ec_stoeber.c
View file @
1ebd504b
...
...
@@ -23,6 +23,70 @@
#include
"esc.h"
#include
"ec_stoeber.h"
#include
"log.h"
static
void
ec_stoeber_emergency_handler
(
struct
canopen_dev
*
dev
,
uint16_t
code
,
uint8_t
reg
,
uint8_t
vendor
[
5
])
{
char
*
msg
=
"unknown"
;
if
(
code
==
0x0000
)
{
msg
=
"no_error"
;
}
else
if
(
code
==
0x2110
)
{
msg
=
"short circuit earth"
;
}
else
if
(
code
==
0x2230
)
{
msg
=
"intern short circuit earth"
;
}
else
if
(
code
==
0x2310
)
{
msg
=
"continuous overcurrent"
;
}
else
if
(
code
==
0x3110
)
{
msg
=
"mains overvoltage"
;
}
else
if
(
code
==
0x3120
)
{
msg
=
"mains undervoltage"
;
}
else
if
(
code
==
0x4210
)
{
msg
=
"temperature device"
;
}
else
if
(
code
==
0x4280
)
{
msg
=
"temperature device i2t"
;
}
else
if
(
code
==
0x4310
)
{
msg
=
"temperature drive"
;
}
else
if
(
code
==
0x4310
)
{
msg
=
"temperature drive i2t"
;
}
else
if
(
code
==
0x5000
)
{
msg
=
"device hardware"
;
}
else
if
(
code
==
0x5200
)
{
msg
=
"device hardware control"
;
}
else
if
(
code
==
0x6010
)
{
msg
=
"software reset"
;
}
else
if
(
code
==
0x6100
)
{
msg
=
"internal software"
;
}
else
if
(
code
==
0x6200
)
{
msg
=
"user software"
;
}
else
if
(
code
==
0x6300
)
{
msg
=
"data record"
;
}
else
if
(
code
==
0x6310
)
{
msg
=
"loss of parameters"
;
}
else
if
(
code
==
0x7110
)
{
msg
=
"brake chopper"
;
}
else
if
(
code
==
0x7120
)
{
msg
=
"motor"
;
}
else
if
(
code
==
0x7303
)
{
msg
=
"resolver 1 fault"
;
}
else
if
(
code
==
0x8100
)
{
msg
=
"communication"
;
}
else
if
(
code
==
0x8400
)
{
msg
=
"velocity speed control"
;
}
else
if
(
code
==
0x8311
)
{
msg
=
"excess torque"
;
}
else
if
(
code
==
0x9000
)
{
msg
=
"external error"
;
}
else
if
(
code
==
0xa000
)
{
msg
=
"Transition from Pre-Operational to Safe-Operational was not successful"
;
}
else
if
(
code
==
0xa001
)
{
msg
=
"Transition from Safe-Operational to Pre-Operational was not successful"
;
}
log_send
(
LOG_T_WARNING
,
"%s: %s"
,
dev
->
logname
,
msg
);
}
int
ec_stoeber_init
(
struct
ec_stoeber
*
stbr
)
{
...
...
@@ -77,8 +141,18 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
mb_wr
.
len
=
0x0040
;
mb_wr
.
sm
=
0
;
mb
=
esc_mailbox_create
(
&
stbr
->
addr
,
&
mb_rd
,
&
mb_wr
);
canopen_dev
=
esc_coe_create
(
mb
);
if
(
stbr
->
canopen_dev
)
{
esc_mailbox_reinit
(
&
stbr
->
addr
,
&
mb_rd
,
&
mb_wr
);
canopen_dev
=
stbr
->
canopen_dev
;
}
else
{
mb
=
esc_mailbox_create
(
&
stbr
->
addr
,
&
mb_rd
,
&
mb_wr
);
canopen_dev
=
esc_coe_create
(
mb
);
canopen_emergency_handler_attach
(
canopen_dev
,
ec_stoeber_emergency_handler
);
stbr
->
canopen_dev
=
canopen_dev
;
}
printf
(
"Going to state pre-operational
\n
"
);
...
...
@@ -279,9 +353,6 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
leval32
=
STOEBER_PDO_MAP
(
'C'
,
230
,
0
,
16
);
canopen_write_param
(
canopen_dev
,
STOEBER_PARAM2INDEX
(
'A'
,
225
),
0x5
,
&
leval32
,
4
);
esc_coe_destroy
(
canopen_dev
);
esc_mailbox_destroy
(
mb
);
printf
(
"Setup PDOs
\n
"
);
...
...
controller/ec/ec_stoeber.h
View file @
1ebd504b
...
...
@@ -54,6 +54,8 @@ struct ec_stoeber {
uint32_t
serial_number
;
struct
canopen_dev
*
canopen_dev
;
/* rx pdo */
unsigned
char
*
rx_buffer
;
/* 14 */
uint32_t
*
rx_I80
;
...
...
controller/ec/esc.c
View file @
1ebd504b
...
...
@@ -20,6 +20,8 @@
#include
<string.h>
#include
<stdio.h>
#include
<time.h>
#include
<pthread.h>
#include
<limits.h>
#include
"esc.h"
#include
"ec.h"
...
...
@@ -35,6 +37,8 @@ struct esc_mailbox {
int
len_wr
;
unsigned
char
*
buffer_rd
;
unsigned
char
*
buffer_wr
;
void
(
*
error_handler
)(
void
*
msg
,
size_t
len
);
};
...
...
@@ -53,6 +57,7 @@ struct esc_mailbox_hdr {
#define ESC_MAILBOX_TYPE_SOE 0x5
#define ESC_MAILBOX_TYPE_VOE 0xf
#define ESC_COE_TIMEOUT 2
ssize_t
esc_esi_eeprom_read
(
struct
ec_dgram_addr
*
ec_addr
,
void
*
buffer
,
size_t
offset
,
size_t
size
)
...
...
@@ -353,6 +358,18 @@ int esc_syncmanager_claim(struct ec_dgram_addr *addr)
return
nr
;
}
static
void
esc_error_handler_default
(
void
*
msg
,
size_t
len
)
{
unsigned
char
*
msgb
=
msg
;
int
i
;
printf
(
"Mailbox error message:"
);
for
(
i
=
0
;
i
<
len
;
i
++
)
{
printf
(
" 0x%02x"
,
msgb
[
i
]);
}
printf
(
"
\n
"
);
}
struct
esc_mailbox
*
esc_mailbox_create
(
struct
ec_dgram_addr
*
node
,
struct
esc_syncmanager_info
*
rd
,
struct
esc_syncmanager_info
*
wr
)
{
...
...
@@ -509,6 +526,7 @@ printf("Read status byte: 0x%02x\n", val8);
if
(
!
mailbox
->
buffer_wr
)
goto
err_buffer_wr
;
}
mailbox
->
error_handler
=
esc_error_handler_default
;
}
return
mailbox
;
...
...
@@ -521,6 +539,149 @@ err_buffer_rd:
return
NULL
;
}
void
esc_mailbox_reinit
(
struct
ec_dgram_addr
*
node
,
struct
esc_syncmanager_info
*
rd
,
struct
esc_syncmanager_info
*
wr
)
{
int
sync_manager_write
,
sync_manager_read
;
size_t
start_write
,
start_read
;
size_t
len_write
,
len_read
;
struct
esc_mailbox
mb
;
uint8_t
val8
;
uint16_t
leval16
;
/* init write sync manager */
if
(
!
wr
)
{
sync_manager_write
=
esc_syncmanager_claim
(
node
);
if
(
sync_manager_write
<
0
)
return
;
start_write
=
0x1000
;
len_write
=
0x0100
;
}
else
{
sync_manager_write
=
wr
->
sm
;
start_write
=
wr
->
start
;
len_write
=
wr
->
len
;
}
printf
(
"Claiming syncmanager %d for mailbox write
\n
"
,
sync_manager_write
);
/* mailbox write (master->slave) */
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_write
)
|
ESC_ADDR_MAP_SYNCMANAGER_ACTIVATE
;
val8
=
0x00
;
ec_datagram_write
(
node
,
&
val8
,
1
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_write
)
|
ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START
;
leval16
=
htole16
(
start_write
);
ec_datagram_write
(
node
,
&
leval16
,
2
);
printf
(
"write address: %04zx
\n
"
,
start_write
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_write
)
|
ESC_ADDR_MAP_SYNCMANAGER_LENGTH
;
leval16
=
htole16
(
len_write
);
ec_datagram_write
(
node
,
&
leval16
,
2
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_write
)
|
ESC_ADDR_MAP_SYNCMANAGER_CONTROL
;
val8
=
ESC_SYNCMANAGER_CONTROL_MAILBOX
|
ESC_SYNCMANAGER_CONTROL_WRITE
|
ESC_SYNCMANAGER_CONTROL_INT_PDI
;
printf
(
"Write Control byte: 0x%02x
\n
"
,
val8
);
ec_datagram_write
(
node
,
&
val8
,
1
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_write
)
|
ESC_ADDR_MAP_SYNCMANAGER_STATUS
;
ec_datagram_read
(
node
,
&
val8
,
1
);
printf
(
"Write status byte: 0x%02x
\n
"
,
val8
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_write
)
|
ESC_ADDR_MAP_SYNCMANAGER_ACTIVATE
;
val8
=
ESC_SYNCMANAGER_ACTIVATE_ENABLE
;
ec_datagram_write
(
node
,
&
val8
,
1
);
/* init read sync manager */
if
(
!
rd
)
{
sync_manager_read
=
esc_syncmanager_claim
(
node
);
if
(
sync_manager_read
<
0
)
return
;
start_read
=
0x1100
;
len_read
=
0x0100
;
}
else
{
sync_manager_read
=
rd
->
sm
;
start_read
=
rd
->
start
;
len_read
=
rd
->
len
;
}
printf
(
"Claiming syncmanager %d for mailbox read
\n
"
,
sync_manager_read
);
/* mailbox read (slave->master) */
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_read
)
|
ESC_ADDR_MAP_SYNCMANAGER_ACTIVATE
;
val8
=
0x00
;
ec_datagram_write
(
node
,
&
val8
,
1
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_read
)
|
ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START
;
leval16
=
htole16
(
start_read
);
ec_datagram_write
(
node
,
&
leval16
,
2
);
printf
(
"read address: %04zx
\n
"
,
start_read
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_read
)
|
ESC_ADDR_MAP_SYNCMANAGER_LENGTH
;
leval16
=
htole16
(
len_read
);
ec_datagram_write
(
node
,
&
leval16
,
2
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_read
)
|
ESC_ADDR_MAP_SYNCMANAGER_CONTROL
;
val8
=
ESC_SYNCMANAGER_CONTROL_MAILBOX
|
ESC_SYNCMANAGER_CONTROL_INT_PDI
;
printf
(
"Read Control byte: 0x%02x
\n
"
,
val8
);
ec_datagram_write
(
node
,
&
val8
,
1
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_read
)
|
ESC_ADDR_MAP_SYNCMANAGER_STATUS
;
ec_datagram_read
(
node
,
&
val8
,
1
);
printf
(
"Read status byte: 0x%02x
\n
"
,
val8
);
node
->
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_read
)
|
ESC_ADDR_MAP_SYNCMANAGER_ACTIVATE
;
val8
=
ESC_SYNCMANAGER_ACTIVATE_ENABLE
;
ec_datagram_write
(
node
,
&
val8
,
1
);
memcpy
(
&
mb
.
status_rd
,
node
,
sizeof
(
struct
ec_dgram_addr
));
mb
.
status_rd
.
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_read
)
|
ESC_ADDR_MAP_SYNCMANAGER_STATUS
;
memcpy
(
&
mb
.
status_wr
,
node
,
sizeof
(
struct
ec_dgram_addr
));
mb
.
status_wr
.
addr
.
position
.
off
=
ESC_ADDR_MAP_SYNCMANAGER_NR
(
sync_manager_write
)
|
ESC_ADDR_MAP_SYNCMANAGER_STATUS
;
mb
.
len_rd
=
len_read
;
memcpy
(
&
mb
.
data_rd
,
node
,
sizeof
(
struct
ec_dgram_addr
));
mb
.
data_rd
.
addr
.
position
.
off
=
start_read
;
mb
.
len_wr
=
len_write
;
memcpy
(
&
mb
.
data_wr
,
node
,
sizeof
(
struct
ec_dgram_addr
));
mb
.
data_wr
.
addr
.
position
.
off
=
start_write
;
}
void
esc_mailbox_destroy
(
struct
esc_mailbox
*
mailbox
)
{
/* todo: deactivate sync managers */
...
...
@@ -536,6 +697,7 @@ ssize_t esc_mailbox_write(struct esc_mailbox *mailbox, int type,
struct
esc_mailbox_hdr
*
hdr
;
uint8_t
val8
;
int
ret
;
time_t
start
=
time
(
NULL
);
if
(
mailbox
->
len_wr
==
0
)
{
printf
(
"len_wr == 0
\n
"
);
...
...
@@ -557,22 +719,22 @@ ssize_t esc_mailbox_write(struct esc_mailbox *mailbox, int type,
mailbox
->
ctr
++
;
/* wait till mailbox is empty */
while
(
1
)
{
do
{
ret
=
ec_datagram_read
(
&
mailbox
->
status_wr
,
&
val8
,
sizeof
(
val8
));
if
(
ret
!=
sizeof
(
val8
))
{
printf
(
"could not read mailbox status
\n
"
);
return
-
1
;
}
}
while
((
val8
&
0x08
)
&&
(
time
(
NULL
)
-
start
<
ESC_COE_TIMEOUT
));
if
(
!
(
val8
&
0x08
))
{
/* write mailbox contents */
ret
=
ec_datagram_write
(
&
mailbox
->
data_wr
,
mailbox
->
buffer_wr
,
mailbox
->
len_wr
);
if
(
ret
>
0
)
{
b
re
ak
;
if
(
ret
!=
mailbox
->
len_wr
)
{
re
turn
-
1
;
}
/* wait while mailbox full */
do
{
ret
=
ec_datagram_read
(
&
mailbox
->
status_wr
,
&
val8
,
sizeof
(
val8
));
if
(
ret
!=
sizeof
(
val8
))
{
printf
(
"could not read mailbox status
\n
"
);
return
-
1
;
}
}
while
(
val8
&
0x08
);
}
return
len
;
...
...
@@ -585,6 +747,7 @@ ssize_t esc_mailbox_read(struct esc_mailbox *mailbox, int *type,
int
ret
;
size_t
clen
;
uint8_t
val8
;
time_t
start
=
time
(
NULL
);
if
(
mailbox
->
len_rd
==
0
)
{
printf
(
"len_rd == 0
\n
"
);
...
...
@@ -598,22 +761,20 @@ ssize_t esc_mailbox_read(struct esc_mailbox *mailbox, int *type,
hdr
=
(
struct
esc_mailbox_hdr
*
)
mailbox
->
buffer_rd
;
/* wait till mailbox is full */
while
(
1
/*todo add timeout*/
)
{
/* wait while mailbox full */
do
{
ret
=
ec_datagram_read
(
&
mailbox
->
status_rd
,
&
val8
,
sizeof
(
val8
));
if
(
ret
!=
sizeof
(
val8
))
{
printf
(
"Could not read mailbox status
\n
"
);
return
-
1
;
}
}
while
(
val8
&
0x08
);
/* read mailbox contents */
ret
=
ec_datagram_read
(
&
mailbox
->
data_rd
,
mailbox
->
buffer_rd
,
mailbox
->
len_rd
);
if
(
ret
>
0
)
{
break
;
/* wait untill mailbox full */
do
{
ret
=
ec_datagram_read
(
&
mailbox
->
status_rd
,
&
val8
,
sizeof
(
val8
));
if
(
ret
!=
sizeof
(
val8
))
{
printf
(
"Could not read mailbox status
\n
"
);
return
-
1
;
}
}
while
(
!
(
val8
&
0x08
)
&&
(
time
(
NULL
)
-
start
<
ESC_COE_TIMEOUT
));
/* read mailbox contents */
ret
=
ec_datagram_read
(
&
mailbox
->
data_rd
,
mailbox
->
buffer_rd
,
mailbox
->
len_rd
);
if
(
ret
!=
mailbox
->
len_rd
)
{
return
-
1
;
}
clen
=
le16toh
(
hdr
->
length
);
...
...
@@ -622,6 +783,12 @@ ssize_t esc_mailbox_read(struct esc_mailbox *mailbox, int *type,
memcpy
(
data
,
mailbox
->
buffer_rd
+
sizeof
(
struct
esc_mailbox_hdr
),
clen
);
*
type
=
hdr
->
type_ctr
&
0x0f
;
if
(
*
type
==
ESC_MAILBOX_TYPE_ERROR
)
{
mailbox
->
error_handler
(
mailbox
->
buffer_rd
+
sizeof
(
struct
esc_mailbox_hdr
),
hdr
->
length
);
}
return
clen
;
}
...
...
@@ -629,16 +796,18 @@ ssize_t esc_mailbox_read(struct esc_mailbox *mailbox, int *type,