static void *timer(UNUSED void *args) {
CRecord *retry, *timed, *ping, *purge, *cr;
int counter = 0;
debugf("timer thread started\n");
for (;;) {
if (nanosleep(&one_tick, NULL) != 0)
break;
ctable_lock();
if ((++counter % 500) == 0) {
counter = 0;
#ifdef VLOG
logvf("Dump of connection table\n");
ctable_dump("LOGV> ");
#endif /* VLOG */
}
ctable_scan(&retry, &timed, &ping, &purge);
while (purge != NULL) {
cr = purge->link;
ctable_remove(purge);
crecord_destroy(purge);
purge = cr;
}
while (timed != NULL) {
cr = timed->link;
crecord_setState(timed, ST_TIMEDOUT);
timed = cr;
}
while (ping != NULL) {
ControlPayload pl;
cr = ping->link;
cp_complete(&pl, ping->ep->subport, PING, ping->seqno, 1, 1);
(void)send_payload(ping->ep, &pl, CP_SIZE);
ping = cr;
}
while (retry != NULL) {
cr = retry->link;
switch(retry->state) {
case ST_CONNECT_SENT:
case ST_QUERY_SENT:
case ST_RESPONSE_SENT:
case ST_DISCONNECT_SENT:
case ST_FRAGMENT_SENT:
case ST_SEQNO_SENT:
(void)send_payload(retry->ep, retry->pl, retry->size);
break;
}
retry = cr;
}
ctable_unlock();
}
return NULL;
}
static void send_effect_names(void) {
send_16(EFFECT_JSON_LEN);
send_payload('[');
for (uint8_t i = 0; i < effects_len; i++) {
send_payload('"');
send_pgm_string(&effects[i].name);
send_payload('"');
if (i < effects_len - 1) {
send_payload(',');
};
}
send_payload(']');
}
static void send_zcl_header(uint8_t cmd) {
send_payload(ZCL_CHANNEL);
send_64(mac);
send_payload(zcl.packet.endpoint);
send_16(zcl.packet.profile);
send_16(zcl.packet.cluster);
// Send out the frame control byte
//FIXME: see if needs to be non-zero
send_payload(0);
send_payload(zcl.packet.transaction_id);
send_payload(cmd);
}
int rpc_response(UNUSED RpcService rps, RpcEndpoint *ep, void *rb,
unsigned len) {
DataPayload *dp;
int ans = 0;
CRecord *cr;
unsigned char *cp = (unsigned char *)rb;
unsigned char fnum, nfrags;
int size, blen;
unsigned long fstates[2] = {ST_FACK_RECEIVED, ST_TIMEDOUT};
ctable_lock();
cr = ctable_look_ep(ep);
if (cr != NULL && cr->state == ST_QACK_SENT) {
nfrags = (len - 1) / FR_SIZE + 1;
for (fnum = 1; fnum < nfrags; fnum++) {
size = sizeof(PayloadHeader) + sizeof(DataHeader) + FR_SIZE;
dp = (DataPayload *)malloc(size);
cp_complete((ControlPayload *)dp, ep->subport, FRAGMENT,
cr->seqno, fnum, nfrags);
dp->dhdr.tlen = htons(len);
dp->dhdr.flen = htons(FR_SIZE);
memcpy(dp->data, &(cp[FR_SIZE*(fnum-1)]), FR_SIZE);
cr->lastFrag = fnum;
crecord_setPayload(cr, dp, size, ATTEMPTS, TICKS);
(void)send_payload(ep, dp, size);
crecord_setState(cr, ST_FRAGMENT_SENT);
if (crecord_waitForState(cr, fstates, 2) == ST_TIMEDOUT) {
ctable_unlock();
return 0;
}
}
blen = len - FR_SIZE * (nfrags - 1);
size = sizeof(PayloadHeader) + sizeof(DataHeader) + blen;
dp = (DataPayload *)malloc(size);
cp_complete((ControlPayload *)dp, ep->subport, RESPONSE,
cr->seqno, fnum, nfrags);
dp->dhdr.tlen = htons(len);
dp->dhdr.flen = htons(blen);
memcpy(dp->data, &(cp[FR_SIZE*(fnum-1)]), blen);
crecord_setPayload(cr, dp, size, ATTEMPTS, TICKS);
(void)send_payload(cr->ep, dp, size);
crecord_setState(cr, ST_RESPONSE_SENT);
ans = 1;
}
ctable_unlock();
return ans;
}
void
send_ima( tNode* pNode, BOOL fSendNow ) {
IAnnexET00IAmAlive ima[sizeof_IAnnexET00IAmAlive_ND];
BYTE cookie[4];
ima[IAnnexET00IAmAlive_Header+IAnnexET00Header_PFLAGS] = 0;
AEPOr_T( ima[IAnnexET00IAmAlive_Header+IAnnexET00Header_PFLAGS], AEPT_TransportMessage );
/*
ima.Header.S = 0;
ima.Header.A = 0;
ima.Header.RES = 0;
*/
ima[IAnnexET00IAmAlive_Header+IAnnexET00Header_TRANSPORT_TYPE] = AEPT00_IAmAlive;
IMASet_P( ima );
hton16( (UINT16)get_ima_interval( pNode ), &ima[IAnnexET00IAmAlive_VALIDITY] );
IMASet_CookieLen( ima, sizeof( UINT32 ) );
hton32( timerGetTimeInMilliseconds(), &cookie[0] );
send_payload(
pNode,
ima,
sizeof_IAmAliveHeader( &ima ),
&cookie[0],
4,
NULL,
0,
FALSE,
FALSE,
fSendNow
);
}
void
send_ack( tNode* pNode, UINT32 seqn, BOOL fSendNow ) {
IAnnexET00Ack ack[sizeof_IAnnexET00Ack_ND+sizeof_IAnnexEAckData];
ack[IAnnexET00Ack_Header+IAnnexET00Header_PFLAGS] = 0;
/*
ack.Header.T = 0;
ack.Header.A = 0;
ack.Header.S = 0;
ack.Header.RES = 0;
*/
ack[IAnnexET00Ack_Header+IAnnexET00Header_TRANSPORT_TYPE] = AEPT00_Ack;
hton16( 1, &ack[IAnnexET00Ack_ACK_COUNT] );
hton24( seqn, &ack[IAnnexET00Ack_ACK+IAnnexEAckData_SEQNUM] );
ack[IAnnexET00Ack_ACK+IAnnexEAckData_RESERVED] = 0;
send_payload(
pNode,
ack,
sizeof_AckHeader( &ack ),
&ack[IAnnexET00Ack_ACK],
sizeof_IAnnexEAckData,
NULL,
0,
FALSE,
FALSE,
fSendNow
);
}
void
send_nack5( tNode* pNode,UINT32 seqn, int oid_length, BYTE* poid, BOOL fSendNow ) {
/* send NACK REASON_5:"ObjectID-payload not supported!" */
IAnnexET00NAckHeader header[sizeof_IAnnexET00NAckHeader];
IAnnexENAckReason reason[sizeof_IAnnexENAckReason_ND];
msaPrintFormat( pNode->pAnnexE->msaType, "Send NAck5(ObjectID-payload not supported, SEQN=%i) to Host(%08x:%i)", seqn, pNode->RemoteHost.nIP, pNode->RemoteHost.nPort );
header[IAnnexET00NAckHeader_Header+IAnnexET00Header_PFLAGS] = 0;
/*
header.Header.T = 0;
header.Header.A = 0;
header.Header.S = 0;
header.Header.RES = 0;
*/
header[IAnnexET00NAckHeader_Header+IAnnexET00Header_TRANSPORT_TYPE] = AEPT00_NAck;
hton16( 1, &header[IAnnexET00NAckHeader_NACK_COUNT] );
hton24( seqn, &reason[IAnnexENAckReason_SEQNUM] );
reason[IAnnexENAckReason_DATA_LENGTH] = (BYTE)(oid_length & 0xff);
hton16( 5, &reason[IAnnexENAckReason_REASON] );
send_payload(
pNode,
header,
sizeof_IAnnexET00NAckHeader,
reason,
sizeof_IAnnexENAckReason_ND,
poid,
oid_length,
FALSE,
FALSE,
fSendNow
);
}
void set_motor_current(float current) {
int32_t index = 0;
uint8_t payload[5];
payload[index++] = COMM_SET_CURRENT ;
buffer_append_int32(payload, (int32_t)(current * 1000), &index);
send_payload(payload, 5);
}
void set_brake_current(float brakeCurrent) {
int32_t index = 0;
uint8_t payload[5];
payload[index++] = COMM_SET_CURRENT_BRAKE;
buffer_append_int32(payload, (int32_t)(brakeCurrent * 1000), &index);
send_payload(payload, 5);
}
/* Function for sending the Payload */
int send_remote_notification(const char *deviceTokenHex, Payload *payload)
{
char messageBuff[MAXPAYLOAD_SIZE];
char tmpBuff[MAXPAYLOAD_SIZE];
char badgenumBuff[3];
strcpy(messageBuff, "{\"aps\":{");
if(payload->message != NULL)
{
strcat(messageBuff, "\"alert\":");
if(payload->actionKeyCaption != NULL)
{
sprintf(tmpBuff, "{\"body\":\"%s\",\"action-loc-key\":\"%s\"},", payload->message, payload->actionKeyCaption);
strcat(messageBuff, tmpBuff);
}
else
{
sprintf(tmpBuff, "{\"%s\"},", payload->message);
strcat(messageBuff, tmpBuff);
}
}
if(payload->badgeNumber > 99 || payload->badgeNumber < 0)
payload->badgeNumber = 1;
sprintf(badgenumBuff, "%d", payload->badgeNumber);
strcat(messageBuff, "\"badge\":");
strcat(messageBuff, badgenumBuff);
strcat(messageBuff, ",\"sound\":\"");
strcat(messageBuff, payload->soundName == NULL ? "default" : payload->soundName);
strcat(messageBuff, "\"}");
int i = 0;
while(payload->dictKey[i] != NULL && i < 5)
{
sprintf(tmpBuff, "\"%s\":\"%s\"", payload->dictKey[i], payload->dictValue[i]);
strcat(messageBuff, tmpBuff);
if(i < 4 && payload->dictKey[i + 1] != NULL)
{
strcat(messageBuff, ",");
}
i++;
}
strcat(messageBuff, "}");
printf("Sending %s\n", messageBuff);
return send_payload(deviceTokenHex, messageBuff, strlen(messageBuff));
}
bool vesc_get_values(mc_values& values) {
uint8_t command[1] = { COMM_GET_VALUES };
uint8_t payload[256];
send_payload(command, 1);
delay(20); //needed, otherwise data is not read
int lenPayload = process_received_msg(payload);
if (lenPayload > 0) {
bool read = process_read_package(payload, values, lenPayload); //returns true if sucessfull
return read;
}
else
{
return false;
}
}
void
i_am_alive_request_received( tNode* pNode, IAnnexET00IAmAlive* pPayload ) {
msaPrintFormat( pNode->pAnnexE->msaType, "IAmAlive request from Host(%08x:%i)!", pNode->RemoteHost.nIP, pNode->RemoteHost.nPort );
IMAClr_P( pPayload );
send_payload(
pNode,
pPayload,
sizeof_IAmAliveHeader( pPayload ),
&pPayload[IAnnexET00IAmAlive_COOKIE],
sizeof_IAmAliveCookie( pPayload ),
NULL,
0,
FALSE,
FALSE,
TRUE
);
}
/* disconnect from target
*/
void rpc_disconnect(RpcConnection rpc) {
CRecord *cr;
ControlPayload *cp;
RpcEndpoint *ep;
//unsigned long states[1] = {ST_TIMEDOUT};
ctable_lock();
if ((cr = ctable_look_id((unsigned long)rpc)) == NULL) {
ctable_unlock();
return;
}
ep = cr->ep;
cp = (ControlPayload *)malloc(CP_SIZE);
cp_complete(cp, ep->subport, DISCONNECT, cr->seqno, 1, 1);
crecord_setPayload(cr, cp, CP_SIZE, ATTEMPTS, TICKS);
(void) send_payload(ep, cp, CP_SIZE);
crecord_setState(cr, ST_DISCONNECT_SENT);
//(void) crecord_waitForState(cr, states, 1);
ctable_unlock();
}
SAT_returnState tx_ecss(tc_tm_pkt *pkt) {
int32_t ret = 0;
uint16_t size = 0;
SAT_returnState res;
if(pkt == NULL){
comms_rf_stats_frame_transmitted(&comms_stats, 0, SATR_ERROR);
return SATR_ERROR;
}
res = pack_pkt(send_buf, pkt, &size);
if(res != SATR_OK){
comms_rf_stats_frame_transmitted(&comms_stats, 0, res);
return ret;
}
ret = send_payload(send_buf, (size_t)size, 0,COMMS_DEFAULT_TIMEOUT_MS);
if(ret < 1){
return SATR_ERROR;
}
free_pkt (pkt);
return SATR_OK;
}
/* {{{ proto string send(string token,string message)
Send the apple_push data format */
PHP_METHOD(apple_push, send)
{
zend_class_entry * _this_ce;
zval * _this_zval = NULL;
const char * x = NULL;
int x_len = 0;
const char * y = NULL;
int y_len = 0;
if (zend_parse_method_parameters(ZEND_NUM_ARGS() TSRMLS_CC, getThis(), "ss", &x, &x_len,&y,&y_len) == FAILURE)
{
return;
}
char tokenbytes[32];
string_to_bytes(x, tokenbytes);
if(send_payload(ssl, tokenbytes, (char*)y,strlen( (char*)y))>=1)
{
RETURN_TRUE;
}
else
{
RETVAL_FALSE;
}
}
/**
****************************************************************************************
* @brief Transmit a boot image to an SPI slave device
****************************************************************************************
*/
void spi_send_image (void)
{
uint8_t mode;
uint8_t crc;
uint8_t header_ack;
uint8_t payload_ack;
spi_booter_pad.pin = SPI_CS_PIN;
spi_booter_pad.port = SPI_GPIO_PORT;
crc = calc_crc(SPI_LENGTH);
mode = 0x00;
spi_init(&spi_booter_pad, SPI_MODE_8BIT, SPI_ROLE_MASTER, SPI_CLK_IDLE_POL_LOW, SPI_PHA_MODE_0, SPI_MINT_DISABLE, SPI_XTAL_DIV_4);
do{
header_ack = send_header(SPI_LENGTH,crc,mode);
if (header_ack)
{
payload_ack = send_payload(SPI_8BIT,SPI_LENGTH);
break;
}
} while((header_ack&payload_ack)!=1);
spi_release();
}
RpcConnection rpc_connect(char *host, unsigned short port,
char *svcName, unsigned long seqno) {
ConnectPayload *buf;
RpcEndpoint *nep;
int len = sizeof(PayloadHeader) + 1; /* room for '\0' */
CRecord *cr;
unsigned long subport;
unsigned long states[2] = {ST_IDLE, ST_TIMEDOUT};
unsigned long id = 0;
ctable_lock();
subport = ctable_newSubport();
nep = rpc_socket(host, port, subport);
if (nep != NULL) {
id = gen_conn_id();
len += strlen(svcName); /* room for svcName */
buf = (ConnectPayload *)malloc(len);
cp_complete((ControlPayload *)buf, nep->subport, CONNECT, seqno, 1, 1);
strcpy(buf->sname, svcName);
cr = crecord_create(nep, seqno);
crecord_setCID(cr, id);
crecord_setPayload(cr, buf, len, ATTEMPTS, TICKS);
#ifdef LOG
dumpsockNpacket(&(nep->addr), (DataPayload *)buf, "rpc_connect");
#endif /* LOG */
(void) send_payload(nep, buf, len);
crecord_setState(cr, ST_CONNECT_SENT);
ctable_insert(cr);
if (crecord_waitForState(cr, states, 2) == ST_TIMEDOUT) {
ctable_remove(cr);
crecord_destroy(cr);
id = 0;
}
ctable_unlock();
}
return (RpcConnection)id;
}
void
send_nack6( tNode* pNode, UINT32 seqn, int nPayload, BOOL fSendNow ) {
/* send NACK REASON_6:"Payload Corrupted" */
IAnnexET00NAckHeader header[sizeof_IAnnexET00NAckHeader];
IAnnexENAckReason6 reason[sizeof_IAnnexENAckReason6];
msaPrintFormat( pNode->pAnnexE->msaType, "Send NAck6(Payload corrupted, Id=%i, SEQN=%i) to Host(%08x:%i)", nPayload, seqn, pNode->RemoteHost.nIP, pNode->RemoteHost.nPort );
header[IAnnexET00NAckHeader_Header+IAnnexET00Header_PFLAGS] = 0;
/*
header.Header.T = 0;
header.Header.A = 0;
header.Header.S = 0;
header.Header.RES = 0;
*/
header[IAnnexET00NAckHeader_Header+IAnnexET00Header_TRANSPORT_TYPE] = AEPT00_NAck;
hton16( 1, &header[IAnnexET00NAckHeader_NACK_COUNT] );
hton24( seqn, &reason[IAnnexENAckReason6_SEQNUM] );
reason[IAnnexENAckReason6_DATA_LENGTH] = 1;
hton16( 6, &reason[IAnnexENAckReason6_REASON] );
reason[IAnnexENAckReason6_PAYLOAD_NUMBER] = (BYTE)nPayload;
send_payload(
pNode,
header,
sizeof_IAnnexET00NAckHeader,
reason,
sizeof_IAnnexENAckReason6,
NULL,
0,
FALSE,
FALSE,
fSendNow
);
}
void
send_nack4( tNode* pNode, UINT32 seqn, int static_type, BOOL fSendNow ) {
/* send NACK REASON_4:"Static-payload type not supported" */
IAnnexET00NAckHeader header[sizeof_IAnnexET00NAckHeader];
IAnnexENAckReason4 reason[sizeof_IAnnexENAckReason4];
msaPrintFormat( pNode->pAnnexE->msaType, "Send NAck4(Static-payload-type=%i not supported, SEQN=%i) to Host(%08x:%i)", static_type, seqn, pNode->RemoteHost.nIP, pNode->RemoteHost.nPort );
header[IAnnexET00NAckHeader_Header+IAnnexET00Header_PFLAGS] = 0;
/*
header.Header.T = 0;
header.Header.A = 0;
header.Header.S = 0;
header.Header.RES = 0;
*/
header[IAnnexET00NAckHeader_Header+IAnnexET00Header_TRANSPORT_TYPE] = AEPT00_NAck;
hton16( 1, &header[IAnnexET00NAckHeader_NACK_COUNT] );
hton24( seqn, &reason[IAnnexENAckReason4_SEQNUM] );
reason[IAnnexENAckReason4_DATA_LENGTH] = 1;
hton16( 4, &reason[IAnnexENAckReason4_REASON] );
reason[IAnnexENAckReason4_STATIC_TYPE] = (BYTE)static_type;
send_payload(
pNode,
header,
sizeof_IAnnexET00NAckHeader,
reason,
sizeof_IAnnexENAckReason4,
NULL,
0,
FALSE,
FALSE,
fSendNow
);
}
// Exploit libsysutils FrameworkListener::dispatchCommand method
int do_fault()
{
return send_payload(update_payload());
}
static void send_attr_resp_header(uint16_t attr, uint8_t type) {
send_16(attr);
send_payload(STATUS_SUCCESS);
send_payload(type);
}
static void send_cmd_status(uint16_t attr, uint8_t status) {
send_16(attr);
send_payload(status);
}
/* continuously reads messages from UDP port */
static void *reader(UNUSED void *args) {
DataPayload *dp;
char buf[10240];
struct sockaddr_in c_addr;
socklen_t len;
int n;
debugf("reader thread started\n");
for(;;) {
unsigned short cmd;
unsigned long sb;
unsigned long seqno;
unsigned char fnum;
unsigned char nfrags;
char *sp;
unsigned short pt;
RpcEndpoint ep;
CRecord *cr;
len = sizeof(c_addr);
memset(&c_addr, 0, len);
n = recvfrom(my_sock, buf, sizeof(buf), 0,
(struct sockaddr *)&c_addr, &len);
if (n < 0)
continue;
buf[n] = '\0';
dp = (DataPayload *)buf;
cmd = ntohs(dp->hdr.command);
sb = ntohl(dp->hdr.subport);
seqno = ntohl(dp->hdr.seqno);
fnum = dp->hdr.fnum;
nfrags = dp->hdr.nfrags;
sp = inet_ntoa(c_addr.sin_addr);
pt = ntohs(c_addr.sin_port);
if (cmd >= CMD_LOW && cmd <= CMD_HIGH) {
logf("%s from %s:%05u:%08lx; seqno = %ld, frag/nfrag = %u/%u\n",
cmdnames[cmd], sp, pt, sb, seqno, fnum, nfrags);
} else {
errorf("Illegal command received: %d\n", cmd);
continue;
}
endpoint_complete(&ep, &c_addr, sb);
ctable_lock();
cr = ctable_look_ep(&ep);
switch (cmd) {
case CONNECT: {
RpcEndpoint *nep;
ConnectPayload *conp = (ConnectPayload *)dp;
ControlPayload *p;
int newcr = 0;
SRecord *sr;
sr = stable_lookup(conp->sname);
if (sr == NULL)
break;
if (cr == NULL) {
nep = endpoint_duplicate(&ep);
cr = crecord_create(nep, seqno);
crecord_setCID(cr, gen_conn_id());
newcr = 1;
} else if (cr->state != ST_IDLE) {
fprintf(stderr,
"%s from %s:%05u:%08lx; seqno = %ld, frag/nfrag = %u/%u\n",
cmdnames[cmd], sp, pt, sb, seqno, fnum, nfrags);
crecord_dump(cr, "connectrqst");
}
if (newcr || cr->state == ST_IDLE) {
if (newcr) {
p = (ControlPayload *)malloc(CP_SIZE);
cp_complete(p, nep->subport, CACK, seqno, 1, 1);
crecord_setPayload(cr, p, CP_SIZE, ATTEMPTS, TICKS);
}
crecord_setService(cr, sr);
(void) send_payload(cr->ep, cr->pl, cr->size);
crecord_setState(cr, ST_IDLE);
}
if (newcr)
ctable_insert(cr);
break;
}
case CACK: {
if ((cr != NULL)) {
if (seqno == cr->seqno)
crecord_setState(cr, ST_IDLE);
}
break;
}
#define NEW 2
#define OLD 1
#define ILL 0
case QUERY: {
DataPayload *p = NULL;
ControlPayload *cp = NULL;
int dplen, cplen;
unsigned long state;
int accept = ILL;
if (cr == NULL)
break;
state = cr->state;
if ((seqno - cr->seqno) == 1 &&
(state == ST_IDLE || state == ST_RESPONSE_SENT)) {
accept = NEW;
cr->seqno = seqno;
p = (DataPayload *)malloc(n);
dplen = n;
memcpy(p, buf, n);
} else if (seqno == cr->seqno && state == ST_FACK_SENT &&
(fnum - cr->lastFrag) == 1 &&
fnum == nfrags) {
void *tp;
unsigned short flen = ntohs(dp->dhdr.flen);
accept = NEW;
p = (DataPayload *)cr->resp;
dplen = sizeof(PayloadHeader) + sizeof(DataHeader) +
ntohs(dp->dhdr.tlen);
cr->resp = NULL;
tp = (void *)&(p->data[FR_SIZE * (fnum - 1)]);
memcpy(tp, dp->data, flen);
} else if (seqno == cr->seqno &&
(state == ST_QACK_SENT || state == ST_RESPONSE_SENT)) {
accept = OLD;
}
switch (accept) {
case NEW:
cplen = CP_SIZE;
cp = (ControlPayload *)malloc(cplen);
cp_complete(cp, ep.subport, QACK, seqno, fnum, nfrags);
crecord_setPayload(cr, cp, cplen, ATTEMPTS, TICKS);
(void)send_payload(cr->ep, cp, cplen);
tsl_append(cr->svc->s_queue, cr->ep, p, dplen);
crecord_setState(cr, ST_QACK_SENT);
break;
case OLD:
(void)send_payload(cr->ep, cr->pl, cr->size);
crecord_setState(cr, state);
break;
case ILL:
break;
}
break;
}
case QACK: {
if (cr != NULL) {
if (seqno == cr->seqno)
crecord_setState(cr, ST_AWAITING_RESPONSE);
}
break;
}
case RESPONSE: {
DataPayload *p = NULL;
ControlPayload *cp = NULL;
int cplen;
unsigned long st;
unsigned short flen = ntohs(dp->dhdr.flen);
if (cr == NULL || seqno != cr->seqno)
break;
st = cr->state;
if (st == ST_QUERY_SENT || st == ST_AWAITING_RESPONSE) {
p = (DataPayload *)malloc(n);
memcpy(p, buf, n);
cr->resp = p;
} else if (st == ST_FACK_SENT && (fnum - cr->lastFrag) == 1 &&
fnum == nfrags) {
p = (DataPayload *)cr->resp;
memcpy(&(p->data[FR_SIZE * (fnum -1)]), dp->data, flen);
cr->lastFrag = fnum;
} else
break;
cplen = CP_SIZE;
cp = (ControlPayload *)malloc(cplen);
cp_complete(cp, ep.subport, RACK, seqno, fnum, nfrags);
crecord_setPayload(cr, cp, cplen, ATTEMPTS, TICKS);
(void)send_payload(cr->ep, cp, cplen);
crecord_setState(cr, ST_IDLE);
break;
}
case RACK: {
if (cr != NULL) {
if (seqno == cr->seqno)
crecord_setState(cr, ST_IDLE);
}
break;
}
case DISCONNECT: {
ControlPayload cp; /* always send a DACK */
cp_complete(&cp, ep.subport, DACK, seqno, 1, 1);
(void)send_payload(&ep, &cp, CP_SIZE);
if (cr != NULL) {
crecord_setState(cr, ST_TIMEDOUT);
}
break;
}
case DACK: {
if (cr != NULL) {
if (seqno == cr->seqno)
crecord_setState(cr, ST_TIMEDOUT);
}
break;
}
case FRAGMENT: {
DataPayload *p = NULL;
ControlPayload *cp = NULL;
int dplen, cplen;
unsigned long st;
int accept = ILL;
unsigned short tlen = ntohs(dp->dhdr.tlen);
unsigned short flen = ntohs(dp->dhdr.flen);
int isQ, isR;
if (cr == NULL)
break;
st = cr->state;
isQ = (st == ST_IDLE || st == ST_RESPONSE_SENT) &&
(seqno - cr->seqno) == 1 && fnum == 1;
isR = (st == ST_QUERY_SENT || st == ST_AWAITING_RESPONSE) &&
seqno == cr->seqno && fnum == 1;
if (isQ || isR) {
accept = NEW;
cr->seqno = seqno;
dplen = sizeof(PayloadHeader) + sizeof(DataHeader) + tlen;
cr->resp = malloc(dplen);
p = (DataPayload *)cr->resp;
memcpy(p, buf, n);
} else if (seqno == cr->seqno && st == ST_FACK_SENT &&
(fnum - cr->lastFrag) == 1) {
void *tp;
accept = NEW;
p = (DataPayload *)cr->resp;
tp = (void *)&(p->data[FR_SIZE * (fnum - 1)]);
memcpy(tp, dp->data, flen);
} else if (seqno == cr->seqno && st == ST_FACK_SENT &&
fnum == cr->lastFrag) {
accept = OLD;
}
switch (accept) {
case NEW:
cr->lastFrag = fnum;
cplen = CP_SIZE;
cp = (ControlPayload *)malloc(cplen);
cp_complete(cp, ep.subport, FACK, seqno, fnum, nfrags);
crecord_setPayload(cr, cp, cplen, ATTEMPTS, TICKS);
(void)send_payload(cr->ep, cp, cplen);
crecord_setState(cr, ST_FACK_SENT);
break;
case OLD:
(void)send_payload(cr->ep, cr->pl, cr->size);
crecord_setState(cr, st);
break;
case ILL:
break;
}
break;
}
case FACK: {
if (cr != NULL) {
if (seqno == cr->seqno && cr->state == ST_FRAGMENT_SENT
&& fnum == cr->lastFrag) {
crecord_setState(cr, ST_FACK_RECEIVED);
}
}
break;
}
case PING: {
ControlPayload cp;
if (cr != NULL) {
cp_complete(&cp, ep.subport, PACK, seqno, 1, 1);
(void)send_payload(&ep, &cp, CP_SIZE);
}
break;
}
case PACK: {
if (cr != NULL) {
crecord_setState(cr, cr->state); /* resets ping data */
}
break;
}
case SEQNO: {
ControlPayload *cp;
if (cr != NULL) {
unsigned long st = cr->state;
if (st == ST_IDLE || st == ST_RESPONSE_SENT) {
cp = (ControlPayload *)malloc(CP_SIZE);
cp_complete(cp, ep.subport, SACK, seqno, 1, 1);
crecord_setPayload(cr, cp, CP_SIZE, ATTEMPTS, TICKS);
(void)send_payload(cr->ep, cp, CP_SIZE);
cr->seqno = seqno;
crecord_setState(cr, ST_IDLE);
}
}
break;
}
case SACK: {
if (cr != NULL && cr->state == ST_SEQNO_SENT) {
crecord_setState(cr, ST_IDLE);
}
break;
}
default: {
break;
}
}
ctable_unlock();
}
return NULL;
}
static bool process_read_cmd() {
send_zcl_header(CMDID_READ_RESPONSE);
while(msg_available()) {
uint16_t attr;
attr = msg_get_16();
if (zcl.packet.cluster == CLUSTERID_BASIC) {
switch(attr) {
case ATTR_DEVICE_ENABLED:
send_attr_resp_header(ATTR_DEVICE_ENABLED, TYPE_BOOLEAN);
send_payload(get_mode());
break;
case ATTR_ALARM_MASK:
send_attr_resp_header(ATTR_ALARM_MASK, TYPE_BOOLEAN);
send_payload(0); //FIXME: implement
break;
default:
send_cmd_status(attr, STATUS_UNSUPPORTED_ATTRIBUTE);
break;
}
} else if (zcl.packet.cluster == CLUSTERID_ELOVALO) {
switch(attr) {
case ATTR_IEEE_ADDRESS:
{
send_attr_resp_header(ATTR_IEEE_ADDRESS, TYPE_IEEE_ADDRESS);
send_64(mac);
break;
}
case ATTR_OPERATING_MODE:
{
send_attr_resp_header(ATTR_OPERATING_MODE, TYPE_ENUM);
send_payload(get_mode());
break;
}
case ATTR_EFFECT_TEXT:
send_cmd_status(ATTR_EFFECT_TEXT, STATUS_WRITE_ONLY);
break;
case ATTR_PLAYLIST:
send_attr_resp_header(ATTR_PLAYLIST, TYPE_UINT8);
send_payload(read_playlist());
break;
case ATTR_TIMEZONE:
send_attr_resp_header(ATTR_TIMEZONE, TYPE_INT32);
send_i32(get_timezone());
break;
case ATTR_TIME:
send_attr_resp_header(ATTR_TIME, TYPE_UTC_TIME);
send_32(time(NULL)-ZIGBEE_TIME_OFFSET);
break;
case ATTR_EFFECT_NAMES:
send_attr_resp_header(ATTR_EFFECT_NAMES,
TYPE_LONG_OCTET_STRING);
send_effect_names();
break;
case ATTR_PLAYLIST_NAMES:
send_attr_resp_header(ATTR_PLAYLIST_NAMES,
TYPE_LONG_OCTET_STRING);
send_16(playlists_json_len);
send_pgm_string_direct(playlists_json);
break;
case ATTR_PLAYLIST_EFFECTS:
{
send_attr_resp_header(ATTR_PLAYLIST_EFFECTS, TYPE_OCTET_STRING);
// current playlist index
uint8_t pl_begin = pgm_get(playlists[active_playlist], byte);
// End index to playlist, not included to playlist
uint8_t pl_end;
if (active_playlist == playlists_len - 1) {
pl_end = master_playlist_len;
} else {
pl_end = pgm_get(playlists[active_playlist + 1], byte);
}
//Send string length
send_payload(pl_end - pl_begin);
for (uint8_t i = pl_begin; i < pl_end; i++) {
send_payload(pgm_get(master_playlist[i].id, byte));
//send_payload(i);
}
break;
}
case ATTR_EFFECT:
send_attr_resp_header(ATTR_EFFECT, TYPE_UINT8);
send_payload(read_effect());
break;
case ATTR_HW_VERSION:
send_attr_resp_header(ATTR_HW_VERSION, TYPE_OCTET_STRING);
send_local_pgm_str(hw_resp);
break;
case ATTR_SW_VERSION:
send_attr_resp_header(ATTR_SW_VERSION, TYPE_OCTET_STRING);
send_local_pgm_str(sw_resp);
break;
case ATTR_PLAYLIST_POSITION:
send_attr_resp_header(ATTR_PLAYLIST_POSITION, TYPE_UINT8);
uint8_t start = pgm_get(playlists[active_playlist],byte);
send_payload(active_effect-start);
break;
default:
send_cmd_status(attr, STATUS_UNSUPPORTED_ATTRIBUTE);
break;
}
} else {
// FIXME: See if correct way to handle incorrect cluster
if (!zcl.packet.disable_def_resp) {
send_default_response(CMDID_READ,
STATUS_UNSUP_CLUSTER_COMMAND);
}
return true;
}
}
return true;
}
int rpc_call(RpcConnection rpc, const struct qdecl *q, unsigned qlen,
void *resp, unsigned rsize, unsigned *rlen) {
DataPayload *buf;
RpcEndpoint *ep;
unsigned short size = sizeof(PayloadHeader) + sizeof(DataHeader) + qlen;
unsigned long seqno;
unsigned long qstates[2] = {ST_IDLE, ST_TIMEDOUT};
unsigned long fstates[2] = {ST_FACK_RECEIVED, ST_TIMEDOUT};
int result = 0;
CRecord *cr;
unsigned char fnum;
unsigned char nfrags;
unsigned blen;
unsigned char *cp = (unsigned char *)q->buf;
if (q->size < (int)qlen) {
fprintf(stderr, "rpc_call() - buffer overrun by caller\n");
return result;
}
ctable_lock();
if ((cr = ctable_look_id((unsigned long)rpc)) == NULL) {
ctable_unlock();
return result;
}
ep = cr->ep;
if (cr->state == ST_IDLE) {
if (cr->seqno >= SEQNO_LIMIT) {
ControlPayload *cp;
cr->seqno = SEQNO_START;
cp = (ControlPayload *)malloc(CP_SIZE);
cp_complete(cp, ep->subport, SEQNO, SEQNO_START, 1, 1);
crecord_setPayload(cr, cp, CP_SIZE, ATTEMPTS, TICKS);
(void)send_payload(ep, cp, CP_SIZE);
crecord_setState(cr, ST_SEQNO_SENT);
if (crecord_waitForState(cr, qstates, 2) == ST_TIMEDOUT) {
ctable_unlock();
return result;
}
}
cr->seqno++;
seqno = cr->seqno;
nfrags = (qlen - 1) / FR_SIZE + 1;
for (fnum = 1; fnum < nfrags; fnum++) {
size = sizeof(PayloadHeader) + sizeof(DataHeader) + FR_SIZE;
buf = (DataPayload *)malloc(size);
cp_complete((ControlPayload *)buf, ep->subport, FRAGMENT,
seqno, fnum, nfrags);
buf->dhdr.tlen = htons(qlen);
buf->dhdr.flen = htons(FR_SIZE);
memcpy(buf->data, &(cp[FR_SIZE*(fnum-1)]), FR_SIZE);
cr->lastFrag = fnum;
crecord_setPayload(cr, buf, size, ATTEMPTS, TICKS);
(void)send_payload(ep, buf, size);
crecord_setState(cr, ST_FRAGMENT_SENT);
if (crecord_waitForState(cr, fstates, 2) == ST_TIMEDOUT) {
ctable_unlock();
return result;
}
}
blen = qlen - FR_SIZE * (nfrags - 1);
size = sizeof(PayloadHeader) + sizeof(DataHeader) + blen;
buf = (DataPayload *)malloc(size);
cp_complete((ControlPayload *)buf, ep->subport, QUERY,
seqno, fnum, nfrags);
buf->dhdr.tlen = htons(qlen);
buf->dhdr.flen = htons(blen);
memcpy(buf->data, &(cp[FR_SIZE*(fnum-1)]), blen);
crecord_setPayload(cr, buf, size, ATTEMPTS, TICKS);
(void)send_payload(ep, buf, size);
crecord_setState(cr, ST_QUERY_SENT);
if (crecord_waitForState(cr, qstates, 2) == ST_TIMEDOUT) {
ctable_unlock();
return result;
}
buf = (DataPayload *)cr->resp;
cr->resp = NULL;
if (cr->state == ST_IDLE) {
size = ntohs(buf->dhdr.tlen);
if (size <= rsize) {
memcpy(resp, buf->data, size);
*rlen = size;
result = 1;
} else
result = 0;
}
free(buf);
}
ctable_unlock();
return result;
}
RVAPI annexEStatus RVCALLCONV
annexESendMessage(
IN HANNEXE hAnnexE,
IN UINT32 ip,
IN UINT16 port,
IN UINT16 CRV,
IN void* message,
IN int size,
IN BOOL fAckHint,
IN BOOL fReplyHint,
IN BOOL fSendHint
) {
tAnnexE *pAnnexE = AsAnnexE( hAnnexE );
tNode *pNode;
if (!pAnnexE)
return annexEStatusBadParameter;
pNode = get_node( pAnnexE, ip, port );
if (pNode == NULL) {
pNode = add_node( pAnnexE, ip, port );
}
if (pNode == NULL) {
msaPrintFormat( pAnnexE->msaType, "annexESendMessage() - annexEStatusResourceProblem!" );
return annexEStatusResourceProblem;
}
if (pNode->fLocalAddressUsed) {
IAnnexET10PayloadSA pheader[sizeof_IAnnexET10PayloadSA_ND];
pheader[IAnnexET10PayloadSA_Header + IAnnexET10Header_PFLAGS] = AEP_MASK_A | AEP_MASK_S | (AEPT_StaticMessage << AEP_SHFT_T);
/*
pheader.Header.A = 1;
pheader.Header.S = 1;
pheader.Header.RES = 0;
pheader.Header.T = AEPT_StaticMessage;
*/
pheader[IAnnexET10PayloadSA_Header + IAnnexET10Header_STATIC_TYPE] = 0; /* h.225 messages */
hton32( pNode->nLocalAddress, &pheader[IAnnexET10PayloadSA_ADDRESS] );
hton16( CRV, &pheader[IAnnexET10PayloadSA_SESSION] );
hton16( (UINT16)size, &pheader[IAnnexET10PayloadSA_DATA_LENGTH] );
if (send_payload( pNode, pheader, sizeof_IAnnexET10PayloadSA_ND, message, size, NULL, 0, fAckHint, fReplyHint, fSendHint )) {
return annexEStatusNormal;
}
else {
return annexEStatusWouldBlock;
}
}
else {
IAnnexET10PayloadS pheader[sizeof_IAnnexET10PayloadS_ND];
pheader[IAnnexET10PayloadS_Header + IAnnexET10Header_PFLAGS] = AEP_MASK_S | (AEPT_StaticMessage << AEP_SHFT_T);
/*
pheader.Header.A = 0;
pheader.Header.S = 1;
pheader.Header.RES = 0;
pheader.Header.T = AEPT_StaticMessage;
*/
pheader[IAnnexET10PayloadS_Header + IAnnexET10Header_STATIC_TYPE] = 0; /* h.225 messages */
hton16( CRV, &pheader[IAnnexET10PayloadS_SESSION] );
hton16( (UINT16)size, &pheader[IAnnexET10PayloadS_DATA_LENGTH] );
if (send_payload( pNode, pheader, sizeof_IAnnexET10PayloadS_ND, message, size, NULL, 0, fAckHint, fReplyHint, fSendHint )) {
return annexEStatusNormal;
}
else {
return annexEStatusWouldBlock;
}
}
}
BOOL
send_payload(
tNode* pNode,
void* payload_header,
int payload_header_size,
void* payload_data,
int payload_data_size,
void* payload_exdata,
int payload_exdata_size,
BOOL AckFlag,
BOOL HFlag,
BOOL SendNow
)
{
tPDU* pCurrentPDU;
BOOL fFirstPayload = FALSE;
/* if can't send add to the list of pended nodes */
if (AckFlag && (pNode->pWaitingForAckPDU != NULL)) {
return FALSE;
}
/* get free PDU if needed */
if (pNode->pCurrentPDU == NULL) {
PLIST_ENTRY plink;
if (IsListEmpty( &pNode->pAnnexE->FreePDUList )) {
InsertTailList( &pNode->pAnnexE->ResBlockedNodeList, &pNode->lPendLink );
return FALSE;
}
fFirstPayload = TRUE;
plink = RemoveHeadList( &pNode->pAnnexE->FreePDUList );
pNode->pCurrentPDU = CONTAINING_RECORD( plink, tPDU, lPDULink );
pCurrentPDU = pNode->pCurrentPDU;
pCurrentPDU->nIP = pNode->RemoteHost.nIP;
pCurrentPDU->nPort = pNode->RemoteHost.nPort;
pCurrentPDU->PDU[IAnnexEHeader_FLAGS] = 0;
hton24( get_next_seqn( pNode ), &pCurrentPDU->PDU[IAnnexEHeader_SEQN] );
pCurrentPDU->nSize = sizeof_IAnnexEHeader;
if (payload_header_size + payload_data_size + payload_exdata_size >= (pCurrentPDU->nMaxSize - pCurrentPDU->nSize)) {
RV_ASSERT( FALSE ); /* this is serious APPLICATION bug (payload is greather than PDU size)!!! */
return FALSE;
}
}
else
pCurrentPDU = pNode->pCurrentPDU;
/* check for free space in PDU */
if (payload_header_size + payload_data_size + payload_exdata_size < (pCurrentPDU->nMaxSize - pCurrentPDU->nSize)) {
/* append payload to the PDU */
if (payload_header_size > 0) {
memcpy( &pCurrentPDU->PDU[pCurrentPDU->nSize], payload_header, payload_header_size );
pCurrentPDU->nSize += payload_header_size;
}
if (payload_data_size > 0) {
memcpy( &pCurrentPDU->PDU[pCurrentPDU->nSize], payload_data, payload_data_size );
pCurrentPDU->nSize += payload_data_size;
}
if (payload_exdata_size > 0) {
memcpy( &pCurrentPDU->PDU[pCurrentPDU->nSize], payload_exdata, payload_exdata_size );
pCurrentPDU->nSize += payload_exdata_size;
}
AEHOr_A( pCurrentPDU->PDU, AckFlag );
AEHOr_H( pCurrentPDU->PDU, HFlag );
if ( (SendNow) && (!pNode->fDontSend) ) {
send_current_pdu( pNode );
}
return TRUE;
}
else {
/* no place for that payload => send current PDU! */
send_current_pdu( pNode );
if (AckFlag && (pNode->pWaitingForAckPDU != NULL))
return FALSE;
else
return send_payload( pNode, payload_header, payload_header_size, payload_data, payload_data_size, payload_exdata, payload_exdata_size, AckFlag, HFlag, SendNow );
}
}
