int CRegEx::grepfile (FILE * fp)
//*************************************************************************
//
// Scan the file for the pattern in pbuf[]
//
//*************************************************************************
{
unsigned int lno = 0, count = 0, n;
char obuf[LMAX];
while (fgets(obuf, LMAX - 2, fp))
{
++lno;
n = regex_match(obuf);
if ((n && ! (m_options & o_v)) || (! n && (m_options & o_v)))
{
++count;
if (! (m_options & o_c))
{
if (m_options & o_n)
putf("%d:", lno);
putf("%s", obuf);
}
}
if ((lno % 50) == 0)
{
waitfor(e_ticsfull);
if (testabbruch()) break;
}
}
if ((m_options & o_c) && ! (m_options & o_x))
putf(ms(m_matchinglines), count);
return count;
}
int MQTTUnsubscribe(MQTTClient* c, const char* topicFilter)
{
int rc = MQTT_FAILURE;
Timer timer;
MQTTString topic = MQTTString_initializer;
topic.cstring = (char *)topicFilter;
int len = 0;
platform_mutex_lock(&c->mutex);
if (!c->isconnected)
goto exit;
platform_timer_init(&timer);
platform_timer_countdown(&timer, c->command_timeout_ms);
if ((len = MQTTSerialize_unsubscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic)) <= 0)
goto exit;
if ((rc = sendPacket(c, len, &timer)) != MQTT_SUCCESS) // send the subscribe packet
goto exit; // there was a problem
if (waitfor(c, UNSUBACK, &timer) == UNSUBACK)
{
unsigned short mypacketid; // should be the same as the packetid above
if (MQTTDeserialize_unsuback(&mypacketid, c->readbuf, c->readbuf_size) == 1)
rc = 0;
}
else
{
rc = MQTT_CONNECTION_LOST;
}
exit:
platform_mutex_unlock(&c->mutex);
return rc;
}
void
Exit(void)
{
while(waitfor(0) != -1)
;
exits("error");
}
int builtin_fg(cmd_t* cmd, job_list_t* jobs) {
int index = -1;
job_t* curr_job;
if (cmd->args[1]) {
// Get job specified.
index = (int) strtol(cmd->args[1], NULL, 10);
curr_job = get_job_by_index(jobs, index);
} else {
// Otherwise get latest job.
curr_job = get_latest_job(jobs);
}
if (curr_job) {
// Wait for process to complete synchronously.
int exit_code = waitfor(curr_job->pid, jobs, 0);
// Extract completed job from job list.
get_job(jobs, curr_job->pid);
return exit_code;
} else {
if (index > 0) {
fprintf(stderr, "fg: job not found: %d\n", index);
} else {
fprintf(stderr, "fg: no current job\n");
}
return 1;
}
}
int MQTTUnsubscribe(MQTTClient* c, const char* topicFilter)
{
int rc = FAILURE;
Timer timer;
MQTTString topic = MQTTString_initializer;
topic.cstring = (char *)topicFilter;
int len = 0;
#if defined(MQTT_TASK)
FreeRTOS_MutexLock(&c->mutex);
#endif
if (!c->isconnected)
goto exit;
TimerInit(&timer);
TimerCountdownMS(&timer, c->command_timeout_ms);
if ((len = MQTTSerialize_unsubscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic)) <= 0)
goto exit;
if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
goto exit; // there was a problem
if (waitfor(c, UNSUBACK, &timer) == UNSUBACK) {
unsigned short mypacketid; // should be the same as the packetid above
if (MQTTDeserialize_unsuback(&mypacketid, c->readbuf, c->readbuf_size) == 1)
rc = 0;
} else
rc = FAILURE;
exit:
#if defined(MQTT_TASK)
FreeRTOS_MutexUnlock(&c->mutex);
#endif
return rc;
}
int MQTTSubscribe(Client* c, const char* topicFilter, enum QoS qos, messageHandler messageHandler, pApplicationHandler_t applicationHandler)
{
int rc = FAILURE;
Timer timer;
int len = 0;
int indexOfFreemessageHandler;
MQTTString topic = MQTTString_initializer;
topic.cstring = (char *)topicFilter;
unsigned char isMessageHandlerFree = 0;
InitTimer(&timer);
countdown_ms(&timer, c->command_timeout_ms);
if (!c->isconnected)
goto exit;
len = MQTTSerialize_subscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic, (int*)&qos);
if (len <= 0)
goto exit;
for (indexOfFreemessageHandler = 0; indexOfFreemessageHandler < MAX_MESSAGE_HANDLERS; ++indexOfFreemessageHandler){
if (c->messageHandlers[indexOfFreemessageHandler].topicFilter == 0){
isMessageHandlerFree = 1;
break;
}
}
if(isMessageHandlerFree == 0){
goto exit;
}
if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
goto exit; // there was a problem
if (waitfor(c, SUBACK, &timer) == SUBACK) // wait for suback
{
int count = 0, grantedQoS = -1;
unsigned short mypacketid;
if (MQTTDeserialize_suback(&mypacketid, 1, &count, &grantedQoS, c->readbuf, c->readbuf_size) == 1)
rc = grantedQoS; // 0, 1, 2 or 0x80
if (rc != 0x80)
{
c->messageHandlers[indexOfFreemessageHandler].topicFilter =
topicFilter;
c->messageHandlers[indexOfFreemessageHandler].fp = messageHandler;
c->messageHandlers[indexOfFreemessageHandler].applicationHandler =
applicationHandler;
rc = 0;
}
}
else
rc = FAILURE;
exit:
DeInitTimer(&timer); //STM : added this line
return rc;
}
int init_mainmenu()
{
static char mmenustr[]= "AT*EAM=\"PC Remote\"\r";
char cmd[MAXLEN];
int ret;
writeport("AT*ECAM=1\r", 10);
if((ret=waitfor("OK", 2, TIMEOUT)) < 1) return ret;
if(strlen(conf->charset))
{
sprintf(cmd, "AT+CSCS=\"%s\"\r",conf->charset);
writeport(cmd, strlen(cmd));
if((ret=waitfor("OK", 2, TIMEOUT)) < 1) return ret;
}
writeport(mmenustr, strlen(mmenustr));
return waitfor("OK", 1, TIMEOUT);
}
int MQTTSubscribeWithResults(MQTTClient *c, const char *topicFilter, enum QoS qos,
messageHandler messageHandler, MQTTSubackData *data)
{
int rc = FAILURE;
Timer timer;
int len = 0;
MQTTString topic = MQTTString_initializer;
topic.cstring = (char *)topicFilter;
#if defined(MQTT_TASK)
MutexLock(&c->mutex);
#endif
if (!c->isconnected) {
goto exit;
}
TimerInit(&timer);
TimerCountdownMS(&timer, c->command_timeout_ms);
len = MQTTSerialize_subscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic, (int *)&qos);
if (len <= 0) {
goto exit;
}
if ((rc = sendPacket(c, len, &timer)) != SUCCESS) { // send the subscribe packet
goto exit; // there was a problem
}
if (waitfor(c, SUBACK, &timer) == SUBACK) { // wait for suback
int count = 0;
unsigned short mypacketid;
data->grantedQoS = QOS0;
if (MQTTDeserialize_suback(&mypacketid, 1, &count, (int *)&data->grantedQoS, c->readbuf, c->readbuf_size) == 1) {
if (data->grantedQoS != 0x80) {
rc = MQTTSetMessageHandler(c, topicFilter, messageHandler);
}
}
} else {
rc = FAILURE;
}
exit:
if (rc == FAILURE) {
MQTTCloseSession(c);
}
#if defined(MQTT_TASK)
MutexUnlock(&c->mutex);
#endif
return rc;
}
void main()
{
sock_init();
/*printf("Opening UDP socket\n");*/
if(!udp_open(&sock, LOCAL_PORT, resolve(REMOTE_IP), REMOTE_PORT, NULL)) {
printf("udp_open failed!\n");
exit(0);
}
/* send heartbeats */
for(;;) {
//putchar('.');
tcp_tick(NULL);
costate {
waitfor(IntervalSec(HEARTBEAT_RATE));
waitfor(send_packet());
}
}
}
/*
* Check the status of switch 2
*/
cofunc void CheckSwitch2()
{
if (S3) //wait for switch S3 press
abort; // if button not down skip out
waitfor(DelayMs(50)); // wait 50 ms
if (S3) //wait for switch S3 press
abort; // if button not still down exit
DS2(ON); // DS2 on
SendMail(1); // send email since button was down 50 ms
DS2(OFF); // DS2 off
while (1)
{
waitfor(S3); // wait for button to go up
waitfor(DelayMs(200)); // wait additional 200 ms
if (S3) //wait for switch S3 press
break; // if button still up break out of while loop
}
}
void main()
{
auto char received;
auto int i;
auto int txconfig;
serBopen(BAUD_RATE);
serCopen(BAUD_RATE);
serBparity(PARAM_OPARITY);
serCparity(PARAM_OPARITY);
txconfig = PARAM_OPARITY;
printf("Starting...\n");
while (1)
{
costate
{
//send as fast as we can
for (i = 0; i < 128; i++)
{
waitfor(DelayMs(10)); //necessary if we are not using
//flow control
waitfordone{ cof_serBputc(i); }
}
//toggle between sending parity bits, and not
if (txconfig)
{
txconfig = 0;
}
else
{
txconfig = PARAM_OPARITY;
}
serBparity(txconfig);
} /* serial B costate.. */
costate
{
//receive characters in a leisurely fashion
waitfordone
{
received = cof_serCgetc();
}
printf("received 0x%x\n", received);
if (serCgetError() & SER_PARITY_ERROR)
{
printf("PARITY ERROR\n");
}
} /* serial C costate.. */
}
}
int getlocal(int addr, int bytes){
rtapi_u32 val = 0;
rtapi_u32 buff;
for (;bytes--;){
buff = READ_LOCAL_CMD | (addr + bytes);
HM2WRITE(remote->command_reg_addr, buff);
waitfor();
HM2READ(remote->data_reg_addr, buff);
val = (val << 8) | buff;
} return val;
}
/*
* Check the status of switch 2
*/
cofunc void CheckSwitch2()
{
if (switchIn(S3)) // wait for switch press
abort; // if button not down skip out
waitfor(DelayMs(50)); // wait 50 ms
if (switchIn(S3)) // wait for switch press
abort; // if button not still down exit
ledOut(DS4, ON); // led on
SendMail(1); // send email since button was down 50 ms
ledOut(DS4, OFF); // led off
while (1)
{
waitfor(switchIn(S3)); // wait for button to go up
waitfor(DelayMs(200)); // wait additional 200 ms
if (switchIn(S3)) // wait for switch press
break; // if button still up break out of while loop
}
}
int fmpdev_hash_final(struct fmp_info *info, struct hash_data *hdata, void *output)
{
int ret;
reinit_completion(&hdata->async.result->completion);
ahash_request_set_crypt(hdata->async.request, NULL, output, 0);
ret = crypto_ahash_final(hdata->async.request);
return waitfor(info, hdata->async.result, ret);
}
/*
* <ring 1> this routine handles DEV_READ and DEV_WRITE message.
*
* @param msg - message
*/
static void hd_rdwt(MESSAGE *msg)
{
int drive = DRV_OF_DEV(msg->DEVICE);
u64 pos = msg->POSITION;
assert((pos >> SECTOR_SIZE_SHIFT) < (1 << 31));
assert((pos & 0x1ff) == 0);
u32 sect_nr = (u32)(pos >> SECTOR_SIZE_SHIFT);
int logidx = (msg->DEVICE - MINOR_hd1a) % NR_SUB_PER_DRIVE;
sect_nr += msg->DEVICE < MAX_PRIM ?
hd_info[drive].primary[msg->DEVICE].base :
hd_info[drive].logical[logidx].base;
struct hd_cmd cmd;
cmd.features = 0;
cmd.count = (msg->CNT + SECTOR_SIZE - 1) / SECTOR_SIZE;
cmd.lba_low = sect_nr & 0xff;
cmd.lba_mid = (sect_nr >> 8) & 0xff;
cmd.lba_high = (sect_nr >> 16) & 0xff;
cmd.device = MAKE_DEVICE_REG(1, drive, (sect_nr >> 24) & 0xf);
cmd.command = (msg->type == DEV_READ) ? ATA_READ : ATA_WRITE;
hd_cmd_out(&cmd);
int bytes_left = msg->CNT;
void *la = (void*)va2la(msg->PROC_NR, msg->BUF);
while (bytes_left > 0)
{
int bytes = min(SECTOR_SIZE, bytes_left);
if (msg->type == DEV_READ)
{
interrupt_wait();
port_read(REG_DATA, hdbuf, SECTOR_SIZE);
phys_copy(la, (void*)va2la(TASK_HD, hdbuf), bytes);
}
else
{
if (!waitfor(STATUS_DRQ, STATUS_DRQ, HD_TIMEOUT))
{
panic("hd writing error.");
}
port_write(REG_DATA, la, bytes);
interrupt_wait();
}
bytes_left -= SECTOR_SIZE;
la += SECTOR_SIZE;
}
}
int setup_start(void){
rtapi_u32 buff=0xF00 | 1 << remote->index;
HM2WRITE(remote->command_reg_addr, buff);
if (waitfor() < 0) return -1;
HM2READ(remote->data_reg_addr, buff);
rtapi_print("setup start: data_reg readback = %x\n", buff);
if (buff & (1 << remote->index)){
rtapi_print("Remote failed to start\n");
return -1;
}
return 0;
}
int doit(void){
rtapi_u32 buff = 0x1000 | (1 << remote->index);
HM2WRITE(remote->command_reg_addr, buff);
if (waitfor() < 0) return -1;
HM2READ(remote->data_reg_addr, buff);
if (buff & (1 << remote->index)){
rtapi_print_msg(RTAPI_MSG_ERR, "Error flag set after CMD Clear %08x\n",
buff);
return -1;
}
return 0;
}
ssize_t fmpdev_hash_update(struct fmp_info *info, struct hash_data *hdata,
struct scatterlist *sg, size_t len)
{
int ret;
reinit_completion(&hdata->async.result->completion);
ahash_request_set_crypt(hdata->async.request, sg, NULL, len);
ret = crypto_ahash_update(hdata->async.request);
return waitfor(info, hdata->async.result, ret);
}
int main()
{
soundEnable();
channel = soundPlayPSG(DutyCycle_50, 10000, 127, 64);
//calls the timerCallBack function 5 times per second.
timerStart(0, ClockDivider_1024, TIMER_FREQ_1024(5), timerCallBack);
waitfor(KEY_A);
return 0;
}
int
c_wait(char **wp)
{
int rv = 0;
int sig;
if (ksh_getopt(wp, &builtin_opt, null) == '?')
return 1;
wp += builtin_opt.optind;
if (*wp == NULL) {
while (waitfor(NULL, &sig) >= 0)
;
rv = sig;
} else {
for (; *wp; wp++)
rv = waitfor(*wp, &sig);
if (rv < 0)
rv = sig ? sig : 127; /* magic exit code: bad job-id */
}
return rv;
}
pid_t
run_shell(int timeout, int nowait)
{
pid_t pid;
char tz[1000];
char *envp[] = {
"TERM=vt100",
"HOME=/",
"PATH=/usr/bin:/bin:/usr/sbin:/sbin",
"SHELL=" SHELL,
"USER=root",
tz,
NULL
};
int sig;
/* Wait for user input */
cprintf("Hit enter to continue...");
if (waitfor(STDIN_FILENO, timeout) <= 0)
return 0;
switch ((pid = fork())) {
case -1:
perror("fork");
return 0;
case 0:
/* Reset signal handlers set for parent process */
for (sig = 0; sig < (_NSIG-1); sig++)
signal(sig, SIG_DFL);
/* Reopen console */
console_init();
/* Pass on TZ */
snprintf(tz, sizeof(tz), "TZ=%s", getenv("TZ"));
/* Now run it. The new program will take over this PID,
* so nothing further in init.c should be run.
*/
execve(SHELL, (char *[]) { "/bin/sh", NULL }, envp);
/* We're still here? Some error happened. */
perror(SHELL);
exit(errno);
default:
if (nowait)
return pid;
else {
waitpid(pid, NULL, 0);
return 0;
}
}
//------------------------------------------------------------------------
// Scroll the Welcome Message
//------------------------------------------------------------------------
nodebug
int scroll_welcome(int x, int y, fontInfo *pInfo, char *ptr, int numBitScroll)
{
static int i, loop, scroll, status;
status = 0;
costate
{
scroll = LCD_XS - (strlen(ptr)*abs(numBitScroll));
for(i=0; i < strlen(ptr); i++)
{
glHScroll(0, 16, LCD_XS, 16, numBitScroll);
waitfor(DelayMs(30));
glPrintf (LCD_XS+numBitScroll, y, pInfo, "%c", ptr[i]);
waitfor(DelayMs(30));
}
glHScroll(0, 16, LCD_XS, 16, -scroll);
waitfor(DelayMs(2500));
status = 1;
}
return(status);
}
// Displays and Controls the System System Menu
int bgMenuSys (void)
{
static int state, done;
auto int options;
state = MENU_INIT;
done = 0;
while (!done)
{
keyProcess();
costate
{
waitfor (( options = glMenu(&MenuSysSetup, &state, 15, 15))!= 0);
switch (options)
{
case 1:
glBackLight(1);
state = MENU_REFRESH;
break;
case 2:
glBackLight(0);
state = MENU_REFRESH;
break;
case 3:
waitfor( bgMenuAdjC() );
state = MENU_REFRESH;
break;
case 4:
done = 1;
break;
}
}
}
glBuffLock();
glMenuClear(&MenuSysSetup); // Clear this menu
glRefreshMenu(&MenuStart); // Refresh the Start Menu
glBuffUnlock();
return 1;
}
int setlocal(int addr, int val, int bytes){
rtapi_u32 b = 0;
rtapi_u32 buff;
int i;
for (i = 0; i < bytes; i++){
b = val & 0xFF;
val >>= 8;
HM2WRITE(remote->data_reg_addr, b);
buff = WRITE_LOCAL_CMD | (addr + i);
HM2WRITE(remote->command_reg_addr, buff);
if (waitfor() < 0) return -1;
}
return 0;
}
PRIVATE void hd_cmd_out(HD_CMD* cmd) {
if (!waitfor(STATUS_BSY, 0, HD_TIMEOUT))
panic("hd error");
out_byte(REG_DEV_CTRL, 0); /* 打开硬盘中断 */
/* 加载参数，依次为features, sector count, lba low, lba mid, lba high, device, command */
out_byte(REG_FEATURES, cmd -> features);
out_byte(REG_NSECTOR, cmd -> count);
out_byte(REG_LBA_LOW, cmd -> lba_low);
out_byte(REG_LBA_MID, cmd -> lba_mid);
out_byte(REG_LBA_HIGH, cmd -> lba_high);
out_byte(REG_DEVICE, cmd -> device);
out_byte(REG_CMD, cmd -> command);
}
int mqtt_subscribe(mqtt_client_t* c, const char* topic, enum mqtt_qos qos, mqtt_message_handler_t handler)
{
int rc = MQTT_FAILURE;
mqtt_timer_t timer;
int len = 0;
mqtt_string_t topicStr = mqtt_string_initializer;
topicStr.cstring = (char *)topic;
mqtt_timer_init(&timer);
mqtt_timer_countdown_ms(&timer, c->command_timeout_ms);
if (!c->isconnected)
goto exit;
len = mqtt_serialize_subscribe(c->buf, c->buf_size, 0, get_next_packet_id(c), 1, &topicStr, (int*)&qos);
if (len <= 0)
goto exit;
if ((rc = send_packet(c, len, &timer)) != MQTT_SUCCESS) // send the subscribe packet
{
goto exit; // there was a problem
}
if (waitfor(c, MQTTPACKET_SUBACK, &timer) == MQTTPACKET_SUBACK) // wait for suback
{
int count = 0, grantedQoS = -1;
unsigned short mypacketid;
if (mqtt_deserialize_suback(&mypacketid, 1, &count, &grantedQoS, c->readbuf, c->readbuf_size) == 1)
rc = grantedQoS; // 0, 1, 2 or 0x80
if (rc != 0x80)
{
int i;
rc = MQTT_FAILURE;
for (i = 0; i < MQTT_MAX_MESSAGE_HANDLERS; ++i)
{
if (c->messageHandlers[i].topicFilter == 0)
{
c->messageHandlers[i].topicFilter = topic;
c->messageHandlers[i].fp = handler;
rc = 0;
break;
}
}
}
}
else
rc = MQTT_FAILURE;
exit:
return rc;
}
// Displays the Start Menu
int bgMenuStart (void)
{
static int state, done;
auto int options;
state = MENU_INIT;
done = 0;
glBuffLock();
glBlankScreen();
glBuffUnlock();
while (!done)
{
keyProcess();
costate
{
waitfor (( options = glMenu(&MenuStart, &state, 0, 0 )) != 0 );
switch (options)
{
case 1:
waitfor ( bgMenuSys() );
state = MENU_REFRESH;
break;
case 2:
waitfor ( bgMenuAna1() );
state = MENU_REFRESH;
break;
case 3:
done = 1;
break;
}
}
}
glBuffLock();
glMenuClear(&MenuStart); // Clear this menu
glBuffUnlock();
return 1;
}
int MQTTUnsubscribe(MQTTClient *c, const char *topicFilter)
{
int rc = FAILURE;
Timer timer;
MQTTString topic = MQTTString_initializer;
topic.cstring = (char *)topicFilter;
int len = 0;
#if defined(MQTT_TASK)
MutexLock(&c->mutex);
#endif
if (!c->isconnected) {
goto exit;
}
TimerInit(&timer);
TimerCountdownMS(&timer, c->command_timeout_ms);
if ((len = MQTTSerialize_unsubscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic)) <= 0) {
goto exit;
}
if ((rc = sendPacket(c, len, &timer)) != SUCCESS) { // send the subscribe packet
goto exit; // there was a problem
}
if (waitfor(c, UNSUBACK, &timer) == UNSUBACK) {
unsigned short mypacketid; // should be the same as the packetid above
if (MQTTDeserialize_unsuback(&mypacketid, c->readbuf, c->readbuf_size) == 1) {
/* remove the subscription message handler associated with this topic, if there is one */
MQTTSetMessageHandler(c, topicFilter, NULL);
}
} else {
rc = FAILURE;
}
exit:
if (rc == FAILURE) {
MQTTCloseSession(c);
}
#if defined(MQTT_TASK)
MutexUnlock(&c->mutex);
#endif
return rc;
}
int MQTTConnect (Client *c, MQTTPacket_connectData *options)
{
Timer connect_timer;
int rc = FAILURE;
int len = 0;
MQTTPacket_connectData default_options = MQTTPacket_connectData_initializer;
InitTimer (&connect_timer);
countdown_ms (&connect_timer, c->command_timeout_ms);
if (c->isconnected) // don't send connect packet again if we are already connected
goto exit;
if (options == 0)
options = &default_options; // set default options if none were supplied
c->keepAliveInterval = options->keepAliveInterval;
countdown (&c->ping_timer, c->keepAliveInterval);
//--------------------------------------------------------------------
// Generate a MQTT "Connect" packet, and send it to the remote Broker
//--------------------------------------------------------------------
len = MQTTSerialize_connect (c->buf, c->buf_size, options);
if (len <= 0)
goto exit; // supplied buffer is too small
rc = sendPacket (c, len, &connect_timer); // send the connect packet
if (rc != SUCCESS)
goto exit; // there was a problem
//--------------------------------------------------------------------
// Wait for and read in the MQTT "ConnAck" reply packet.
//--------------------------------------------------------------------
// this will be a blocking call, wait for the connack
if (waitfor(c, CONNACK, &connect_timer) == CONNACK)
{
unsigned char connack_rc = 255;
char sessionPresent = 0;
if (MQTTDeserialize_connack((unsigned char*) &sessionPresent, &connack_rc,
c->readbuf, c->readbuf_size) == 1)
rc = connack_rc;
else rc = FAILURE;
}
else rc = FAILURE;
exit:
if (rc == SUCCESS)
c->isconnected = 1;
return rc;
}
int MQTTSubscribe (Client *c, const char *topicFilter, enum QoS qos,
messageHandler messageHandler)
{
int i;
int rc = FAILURE;
Timer timer;
int len = 0;
MQTTString topic = MQTTString_initializer;
topic.cstring = (char*) topicFilter;
InitTimer (&timer);
countdown_ms (&timer, c->command_timeout_ms); // default is 1 second timeouts
if ( ! c->isconnected)
goto exit;
len = MQTTSerialize_subscribe (c->buf, c->buf_size, 0, getNextPacketId(c), 1,
&topic, (int*) &qos);
if (len <= 0)
goto exit;
if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
goto exit; // there was a problem
if (waitfor(c, SUBACK, &timer) == SUBACK) // wait for suback
{
int count = 0, grantedQoS = -1;
unsigned short mypacketid;
if (MQTTDeserialize_suback(&mypacketid, 1, &count, &grantedQoS, c->readbuf, c->readbuf_size) == 1)
rc = grantedQoS; // will be 0, 1, 2 or 0x80
if (rc != 0x80)
{
for (i = 0; i < MAX_MESSAGE_HANDLERS; ++i)
{
if (c->messageHandlers[i].topicFilter == 0)
{
c->messageHandlers[i].topicFilter = topicFilter;
c->messageHandlers[i].fp = messageHandler;
rc = 0; // denote success
break;
}
}
}
}
else rc = FAILURE; // timed out - no SUBACK received
exit:
return rc;
}
