/*
void peer_arbitrate()
setup the initial kernel state by monitoring hot-standby port
and IO bus activities.
*/
LOCAL void peer_arbitrate()
{
int cookie;
f8_u8 st;
int i;
struct peer_cmd_t tp;
for(i=0;i<3;i++){
cookie = peer_lock();
set_state(hsp_s_arbitrate);
tp.cmd = Pr_query_status;
tp.code = F8_SUCCESS;
tp.time = ke_get_time();
//peer_flush();
peer_write(&tp, sizeof tp);
peer_unlock(cookie);
miliSleep(HSP_ARBITRATION_TIMEOUT);
/* is there an answer? */
cookie = peer_lock();
st = get_state();
set_state(hsp_s_idle);
peer_unlock(cookie);
if(st==hsp_s_complete)
break;
}
// mode_show();
// logMsg("Peer is %s, state=%s\n", ke_get_mode_name(peerState, state_name(st)));
if(st == hsp_s_complete){
switch(peerState){
case KERN_S_RUNNING:
ki_switch_to(KERN_S_STANDBY);
return;
case KERN_S_ARBITRATING:
if(check_priority())
ki_switch_to(KERN_S_RUNNING);
else
ki_switch_to(KERN_S_STANDBY);
break;
default:
ki_switch_to(KERN_S_RUNNING);
break;
}
}else{
ki_switch_to(KERN_S_RUNNING);
}
hsp_log(("arbitration finished, mode is:%d\n", kernelState));
// ki_switch_to(KERN_S_HALTED);
}
void _set_state(enum hsp_state_t s,const char * fnc, int line)
{
if(s == _state){
return;
}
#if defined(HSP_DEBUG)
{
ktime_t t;
t = ke_get_time() - tBase;
hsp_log(("%d, state change %s->%s, file %s,line %d\n", (int)(t/10000),state_name(_state),state_name(s),fnc,line));
}
#endif
_state = s;
}
LOCAL int IUdpPeer_write(IPeer * _this, const void * buf, int len)
{
IUdpPeer * __this = __dcast__(IPeer, IUdpPeer, _this);
if(__this->vbus){
#if defined(HSP_DEBUG)
static int lastCmd;
static int lastCount;
struct peer_cmd_t * tp;
tp = (struct peer_cmd_t*)buf;
if(tp->cmd == lastCmd){
lastCount++;
}else{
hsp_log(("send cmd:%04x, %d times\n", lastCmd, lastCount));
lastCmd = tp->cmd;
lastCount = 1;
}
/* for sniffer use */
if(g_kernel.status.state != KERN_S_STANDBY){
memcpy((void*)buf,"PRIMRY",6);
if(((struct peer_cmd_t*)buf)->cmd == 0x8002)
__asm int 3;
}else{
LOCAL void peer_rx_event(struct peer_cmd_t * tp, int len)
{
#if 0
logMsg("%d bytes from 81x9\n", len);
#endif
#if defined(HSP_DEBUG)
#if 0
{
static int lastCmd;
static int lastCount;
if(tp->cmd == lastCmd){
lastCount++;
}else{
hsp_log(("got cmd:%04x, %d times,code=%d,p0=%x,p1=%x\n", lastCmd, lastCount,tp->code,tp->params[0],tp->params[1]));
lastCmd = tp->cmd;
lastCount = 1;
}
}
#else
hsp_log(("got cmd:%04x,state=%d,code=%d,p0=%x,p1=%x\n", tp->cmd,kernelState,tp->code,tp->params[0],tp->params[1]));
#endif
#endif
peerCounters[0]++;
hspTimeOffset = tp->time - g_kernel.peer->lastMsgTime;
if(tp->cmd == Pr_query_status){
tp->cmd |= Pr_ack;
tp->code = F8_SUCCESS;
tp->time = ke_get_time();
peer_write3(tp, sizeof *tp, &g_kernel.status, sizeof g_kernel.status, &peerHdr, sizeof peerHdr);
return;
}else if(tp->cmd == (Pr_query_status | Pr_ack)){
if(len == sizeof *tp + sizeof(g_kernel.peer_status) + sizeof(peerHdr)){
memcpy(&g_kernel.peer_status, &tp[1], sizeof g_kernel.peer_status);
if(get_state() == hsp_s_arbitrate){
set_state(hsp_s_complete);
}
}
return;
}
#if 0
if(m.verbose)
logMsg("cmd %d,len %d,offset 0x%x\n", tp->cmd,tp->params[1], tp->params[0]);
#endif
switch(kernelState){
case KERN_S_RUNNING:
primary_on_packet(tp, len);
break;
case KERN_S_STANDBY:
standby_on_packet(tp, len);
break;
case KERN_S_ARBITRATING:
// logMsg("Arbitrating...\n");
break;
default:
/* ignore it */
;
}
}
LOCAL void standby_on_packet(struct peer_cmd_t * tp, int len)
{
char * data;
int dataLen;
data = (char*)&tp[1];
dataLen = len - sizeof(*tp);
switch(get_state()){
case hsp_s_idle:
/* respond to connection command */
if(tp->cmd != Pr_connect){
/* invalid command, stay in in-active state */
break;
}
if(dataLen != sizeof(g_kernel.peer_status)+sizeof peerHdr){
tp->cmd = Pr_connect | Pr_ack;
tp->code = F8_INVALID_DATA;
tp->time = ke_get_time();
peer_write2(tp, sizeof(*tp), &g_kernel.status, sizeof(g_kernel.status));
break;
}
memcpy(&g_kernel.peer_status, data, sizeof(g_kernel.peer_status));
#if 0
if(ke_get_peer_flag(FKERN_LED_SOFT_LOCK) || kernelState==KERN_S_STOPPED){
/* no sync phase is performed while either controller is under controll
of a configuration agent */
tp->code = F8_DATABASE_LOCKED;
set_state(hsp_s_aborted);
}else
#endif
if(g_kernel.status.prog_id == g_kernel.peer_status.prog_id){
memcpy(&peerHdr, data+sizeof g_kernel.status, sizeof peerHdr);
tp->code = F8_SUCCESS;
/* ok, establish connection */
/* start new session */
lastFrameOffset = 0;
lastFrameLength = 0;
set_state(hsp_s_active);
}else{
tp->code = F8_VERSION_MISMATCH;
set_state(hsp_s_aborted);
}
if(peerHdr.zipped_data_len > sizeof(peerDataZipped)){
tp->code=F8_LOW_MEMORY;
set_state(hsp_s_aborted);
}
tp->cmd |= Pr_ack;
tp->time = ke_get_time();
peer_write2(tp, sizeof(*tp), &g_kernel.status, sizeof(g_kernel.status));
break;
case hsp_s_active:
/* the active state, ready to accept data */
if(tp->cmd == Pr_send_volatile){
/* upload data request */
#if 0
hsp_log(("got piece %08x,length=%d,we have %08x/%d\n", tp->params[0], tp->params[1],lastFrameOffset,lastFrameLength));
#endif
if(tp->params[0] != lastFrameOffset+lastFrameLength && tp->params[0] != lastFrameOffset){
/* out of synchronization, terminate session */
hsp_log(("Out of synchronization,%x,expected %x\n", tp->params[0], lastFrameOffset+lastFrameLength));
/* dumpBuf(e, len); */
tp->cmd = Pr_abort;
tp->code = F8_OUT_OF_SYNCHRONIZATION;
tp->time = ke_get_time();
peer_write(tp, sizeof(*tp));
peerCounters[4] = tp->params[0];
set_state(hsp_s_out_of_sync);
break;
}
if(tp->params[0] == lastFrameOffset+lastFrameLength){
if(tp->params[0]+tp->params[1] > peerHdr.zipped_data_len){
hsp_log(("Out of synchronization, out of memory\n"));
/* dumpBuf(e, len); */
tp->cmd = Pr_abort;
tp->code = F8_LOW_MEMORY;
tp->time = ke_get_time();
peer_write(tp, sizeof(*tp));
set_state(hsp_s_out_of_sync);
break;
}
/* ok quite valid packet*/
lastFrameOffset = tp->params[0];
lastFrameLength = tp->params[1];
memcpy(peerDataZipped+lastFrameOffset, data, lastFrameLength);
}
tp->cmd |= Pr_ack;
tp->code = F8_SUCCESS;
tp->time = ke_get_time();
tp->params[0] = lastFrameOffset;
tp->params[1] = lastFrameLength;
if(peer_write(tp, sizeof *tp) < 0){
set_state(hsp_s_linedown);
break;
}
}else if(tp->cmd == Pr_abort){
/* client has terminated session */
set_state(hsp_s_aborted);
}else if(tp->cmd == Pr_disconnect){
if(lastFrameOffset + lastFrameLength == peerHdr.zipped_data_len){
set_state(hsp_s_complete);
}else{
set_state(hsp_s_aborted);
}
}else{
/* invalid command, ignore it */
hsp_log(("out of synchronization, bad command %04x, expecting %x\n", tp->cmd,
lastFrameOffset+lastFrameLength));
set_state(hsp_s_out_of_sync);
}
break;
default:
;
break;
}
}
/*
primary site fsm
*/
LOCAL void primary_on_packet(struct peer_cmd_t * tp, int len)
{
char * data;
int dataLen;
static int bytesLeft;
data = (char*)&tp[1];
dataLen = len - sizeof(*tp);
if(dataLen < 0){
return;
}
if(tp->cmd == Pr_connect || tp->cmd == Pr_send_volatile){
tp->cmd = Pr_abort;
tp->code = F8_COLLISION;
tp->time = ke_get_time();
peer_write(tp, sizeof *tp);
set_state(hsp_s_collision);
return;
}
/* switch-case structure is the classical form of a software fsm
implementation.
*/
switch(get_state()){
case hsp_s_connecting:
if(tp->cmd == (Pr_connect | Pr_ack)){
if(dataLen >= sizeof(g_kernel.peer_status)){
memcpy(&g_kernel.peer_status, data, sizeof g_kernel.peer_status);
}
if(F8_FAILED(tp->code)){
if(tp->code == F8_COLLISION){
set_state(hsp_s_collision);
}else{
set_state(hsp_s_aborted);
}
break;
}
/* connection accepted, send first data packet */
tp->cmd = Pr_send_volatile;
tp->code = F8_SUCCESS;
tp->seq = 0;
tp->ack = 0;
bytesLeft = peerHdr.zipped_data_len;
if(bytesLeft < PEER_RTU - sizeof *tp){
lastFrameLength = bytesLeft;
}else{
lastFrameLength = PEER_RTU - sizeof *tp;
}
tp->params[0] = lastFrameOffset = 0;
tp->params[1] = lastFrameLength;
tp->time = ke_get_time();
if(peer_write2(tp, sizeof(*tp),peerDataZipped,lastFrameLength) == 0){
set_state(hsp_s_active);
}else{
set_state(hsp_s_linedown);
}
hsp_log(("connection built, sending first packet %d bytes.\n", lastFrameLength));
}
break;
case hsp_s_active:
/* */
if(tp->cmd == (Pr_send_volatile | Pr_ack)){
if(tp->params[0] != lastFrameOffset){
/* out of sync */
hsp_log(("out of sync, bad offset %08x, expected %08x\n", tp->params[0], lastFrameOffset));
peerCounters[4] = tp->params[0];
set_state(hsp_s_out_of_sync);
break;
}
lastFrameOffset += lastFrameLength;
tp->params[0] = lastFrameOffset;
bytesLeft -= lastFrameLength;
if(!bytesLeft){
tp->cmd = Pr_disconnect;
tp->code = F8_SUCCESS;
tp->time = ke_get_time();
/* tp->seq = 0x12345678; magic */
peer_write(tp, sizeof(*tp));
set_state(hsp_s_complete);
peerCounters[2]++;
break;
}
if(bytesLeft < PEER_RTU - sizeof *tp){
lastFrameLength = bytesLeft;
}else{
lastFrameLength = PEER_RTU - sizeof *tp;
}
tp->cmd = Pr_send_volatile;
tp->code = F8_SUCCESS;
tp->params[1] = lastFrameLength;
tp->time = ke_get_time();
if(peer_write2(tp,sizeof(*tp),peerDataZipped+lastFrameOffset,lastFrameLength) < 0){
set_state(hsp_s_linedown);
}
}else if(tp->cmd == Pr_abort){
if(tp->code == F8_COLLISION){
set_state(hsp_s_collision);
}else{
set_state(hsp_s_aborted);
}
}else{
hsp_log(("out of sync, bad command %04x\n", tp->cmd));
set_state(hsp_s_out_of_sync);
}
break;
default:
/* only query_status cmd is processed, other cmds are ignored */
;
break;
}
}
