int PubSub_mbed::publish(char* pub_topic, char* msg) {
uint8_t var_header_pub[strlen(pub_topic)+3];
strcpy((char *)&var_header_pub[2], pub_topic);
var_header_pub[0] = 0;
var_header_pub[1] = strlen(pub_topic);
var_header_pub[sizeof(var_header_pub)-1] = 0;
uint8_t fixed_header_pub[] = {MQTTPUBLISH,sizeof(var_header_pub)+strlen(msg)};
uint8_t packet_pub[sizeof(fixed_header_pub)+sizeof(var_header_pub)+strlen(msg)];
memset(packet_pub,0,sizeof(packet_pub));
memcpy(packet_pub,fixed_header_pub,sizeof(fixed_header_pub));
memcpy(packet_pub+sizeof(fixed_header_pub),var_header_pub,sizeof(var_header_pub));
memcpy(packet_pub+sizeof(fixed_header_pub)+sizeof(var_header_pub),msg,strlen(msg));
//Publish message
err_t err = tcp_write(this->pcb, (void *)packet_pub, sizeof(packet_pub), 1); //TCP_WRITE_FLAG_MORE
if (err == ERR_OK) {
tcp_output(this->pcb);
printf("Publish: %s ...\r\n", msg);
} else {
printf("Failed to publish...\r\n");
printf("Error is: %d\r\n",err);
tcp_close(this->pcb);
return -1;
}
printf("\r\n");
device_poll();
return 1;
}
void PubSub_mbed::subscribe(char* topic) {
if (connected) {
uint8_t var_header_topic[] = {0,10};
uint8_t fixed_header_topic[] = {MQTTSUBSCRIBE,sizeof(var_header_topic)+strlen(topic)+3};
// utf topic
uint8_t utf_topic[strlen(topic)+3];
strcpy((char *)&utf_topic[2], topic);
utf_topic[0] = 0;
utf_topic[1] = strlen(topic);
utf_topic[sizeof(utf_topic)-1] = 0;
char packet_topic[sizeof(var_header_topic)+sizeof(fixed_header_topic)+strlen(topic)+3];
memset(packet_topic,0,sizeof(packet_topic));
memcpy(packet_topic,fixed_header_topic,sizeof(fixed_header_topic));
memcpy(packet_topic+sizeof(fixed_header_topic),var_header_topic,sizeof(var_header_topic));
memcpy(packet_topic+sizeof(fixed_header_topic)+sizeof(var_header_topic),utf_topic,sizeof(utf_topic));
//Send message
err_t err = tcp_write(this->pcb, (void *)packet_topic, sizeof(packet_topic), 1); //TCP_WRITE_FLAG_MORE
if (err == ERR_OK) {
tcp_output(this->pcb);
printf("Subscribe sucessfull to: %s...\r\n", topic);
} else {
printf("Failed to subscribe to: %s...\r\n", topic);
printf("Error is: %d\r\n",err);
tcp_close(pcb);
}
printf("\r\n");
device_poll();
}
}
int PubSub_mbed::connect(char *id) {
err_t err= tcp_connect(this->pcb, &server, 1883, tcp_fun);
if(err != ERR_OK) {
printf("Connection Error : %d\r\n",err);
return -1;
} else {
//printf("Connection sucessed..\r\n");
}
wait(1);
tcp_accept(this->pcb, tcp_fun);
device_poll();
// variable header
uint8_t var_header[] = {0x00,0x06,0x4d,0x51,0x49,0x73,0x64,0x70,0x03,0x02,0x00,KEEPALIVE/500,0x00,strlen(id)};
// fixed header: 2 bytes, big endian
uint8_t fixed_header[] = {MQTTCONNECT,12+strlen(id)+2};
char packet[sizeof(fixed_header)+sizeof(var_header)+strlen(id)];
memset(packet,0,sizeof(packet));
memcpy(packet,fixed_header,sizeof(fixed_header));
memcpy(packet+sizeof(fixed_header),var_header,sizeof(var_header));
memcpy(packet+sizeof(fixed_header)+sizeof(var_header),id,strlen(id));
//Send MQTT identification message to broker.
err = tcp_write(this->pcb, (void *)packet, sizeof(packet), 1);
if (err == ERR_OK) {
tcp_output(this->pcb);
//printf("Identificaiton message sended correctlly...\r\n");
return 1;
} else {
printf("Failed to send the identification message to broker...\r\n");
printf("Error is: %d\r\n",err);
tcp_close(this->pcb);
return -2;
}
printf("\r\n");
device_poll();
return 1;
}
int cmd_run(char **arg)
{
device_status_t status;
address_t regs[DEVICE_NUM_REGS];
(void)arg;
if (device_getregs(regs) < 0) {
printc_err("warning: device: can't fetch registers\n");
} else {
int i;
for (i = 0; i < device_default->max_breakpoints; i++) {
struct device_breakpoint *bp =
&device_default->breakpoints[i];
if ((bp->flags & DEVICE_BP_ENABLED) &&
bp->type == DEVICE_BPTYPE_BREAK &&
bp->addr == regs[0])
break;
}
if (i < device_default->max_breakpoints) {
printc("Stepping over breakpoint #%d at 0x%04x\n",
i, regs[0]);
device_ctl(DEVICE_CTL_STEP);
}
}
if (device_ctl(DEVICE_CTL_RUN) < 0) {
printc_err("run: failed to start CPU\n");
return -1;
}
printc("Running. Press Ctrl+C to interrupt...\n");
do {
status = device_poll();
} while (status == DEVICE_STATUS_RUNNING);
if (status == DEVICE_STATUS_INTR)
printc("\n");
if (status == DEVICE_STATUS_ERROR)
return -1;
if (device_ctl(DEVICE_CTL_HALT) < 0)
return -1;
return cmd_regs(NULL);
}
static int run(struct gdb_data *data, char *buf)
{
printc("Running\n");
if (run_set_pc(data, buf) < 0 ||
device_ctl(DEVICE_CTL_RUN) < 0)
return gdb_send(data, "E00");
for (;;) {
device_status_t status = device_poll();
if (status == DEVICE_STATUS_ERROR)
return gdb_send(data, "E00");
if (status == DEVICE_STATUS_HALTED) {
printc("Target halted\n");
goto out;
}
if (status == DEVICE_STATUS_INTR)
goto out;
while (gdb_peek(data, 0)) {
int c = gdb_getc(data);
if (c < 0)
return -1;
if (c == 3) {
printc("Interrupted by gdb\n");
goto out;
}
}
}
out:
if (device_ctl(DEVICE_CTL_HALT) < 0)
return gdb_send(data, "E00");
return run_final_status(data);
}
