int send_defined_can_frame(int can_socket, unsigned char *data, int verbose) {
struct can_frame frame;
uint32_t can_id;
memset(&frame, 0, sizeof(frame));
memcpy(&can_id, &data[0], 4);
frame.can_id = htonl(can_id);
frame.can_dlc = data[4];
memcpy(&frame.data, &data[5], 8);
send_can_frame(can_socket, &frame, verbose);
return 0;
}
int get_ms2_loco_names(struct trigger_t *trigger, uint8_t start, int8_t end) {
struct can_frame frame;
memset(&frame, 0, sizeof(frame));
/* get Config Data */
frame.can_id = 0x00400300;
/* first frame */
frame.can_dlc = 8;
memcpy(frame.data, GET_MS2_LOCO_NAMES, sizeof(frame.data));
if (send_can_frame(trigger->socket, &frame, trigger->verbose) < 0)
return (-1);
/* second frame */
memset(frame.data, 0, sizeof(frame.data));
frame.data[0] = start + '0';
frame.data[1] = ' ';
frame.data[2] = end + '0';
if (send_can_frame(trigger->socket, &frame, trigger->verbose) < 0)
return (-1);
return 0;
}
int get_ms2_locoinfo(struct trigger_t *trigger, char *loco_name) {
struct can_frame frame;
memset(&frame, 0, sizeof(frame));
/* get Config Data */
frame.can_id = 0x00400300;
frame.can_dlc = 8;
memcpy(frame.data, GET_MS2_CONFIG_LOCO_I, sizeof(frame.data));
if (send_can_frame(trigger->socket, &frame, trigger->verbose) < 0)
return (-1);
memset(frame.data, 0, 8);
strncpy((char *)frame.data, loco_name, 8);
if (send_can_frame(trigger->socket, &frame, trigger->verbose) < 0)
return (-1);
memset(frame.data, 0, 8);
if (strnlen(loco_name, 16) > 8)
strncpy((char *)frame.data, &loco_name[8], 8);
if (send_can_frame(trigger->socket, &frame, trigger->verbose) < 0)
return (-1);
return 0;
}
int main(int argc, char **argv) {
pthread_t pth;
int opt;
struct ifreq ifr;
struct trigger_t trigger_data;
struct sockaddr_can caddr;
fd_set readfds, exceptfds;
struct can_frame frame;
uint16_t member;
uint8_t buffer[MAXLEN];
memset(&trigger_data, 0, sizeof(trigger_data));
memset(ifr.ifr_name, 0, sizeof(ifr.ifr_name));
strcpy(ifr.ifr_name, "can0");
trigger_data.led_pin = -1;
trigger_data.pb_pin = -1;
trigger_data.interval = DEF_INTERVAL;
while ((opt = getopt(argc, argv, "i:l:p:t:fvh?")) != -1) {
switch (opt) {
case 'i':
strncpy(ifr.ifr_name, optarg, sizeof(ifr.ifr_name));
break;
case 't':
trigger_data.interval = atoi(optarg);
break;
case 'l':
trigger_data.led_pin = atoi(optarg);
break;
case 'p':
trigger_data.pb_pin = atoi(optarg);
break;
case 'v':
trigger_data.verbose = 1;
break;
case 'f':
trigger_data.background = 0;
break;
case 'h':
case '?':
usage(basename(argv[0]));
exit(EXIT_SUCCESS);
default:
usage(basename(argv[0]));
exit(EXIT_FAILURE);
}
}
/* prepare CAN socket */
if ((trigger_data.socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
fprintf(stderr, "error creating CAN socket: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
memset(&caddr, 0, sizeof(caddr));
caddr.can_family = AF_CAN;
if (ioctl(trigger_data.socket, SIOCGIFINDEX, &ifr) < 0) {
fprintf(stderr, "setup CAN socket error: %s %s\n", strerror(errno), ifr.ifr_name);
exit(EXIT_FAILURE);
}
caddr.can_ifindex = ifr.ifr_ifindex;
if (bind(trigger_data.socket, (struct sockaddr *)&caddr, sizeof(caddr)) < 0) {
fprintf(stderr, "error binding CAN socket: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
trigger_data.caddr = caddr;
/* Create thread if LED pin defined */
if ((trigger_data.led_pin) > 0) {
trigger_data.led_pattern = LED_ST_HB_SLOW;
gpio_export(trigger_data.led_pin);
gpio_direction(trigger_data.led_pin, 0);
if (pthread_mutex_init(&lock, NULL)) {
fprintf(stderr, "can't init mutex %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (pthread_create(&pth, NULL, LEDMod, &trigger_data)) {
fprintf(stderr, "can't create pthread %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (!trigger_data.background && trigger_data.verbose)
printf("created LED thread\n");
}
if (trigger_data.background) {
pid_t pid;
/* fork off the parent process */
pid = fork();
if (pid < 0) {
exit(EXIT_FAILURE);
}
/* if we got a good PID, then we can exit the parent process. */
if (pid > 0) {
if (trigger_data.verbose)
printf("Going into background ...\n");
exit(EXIT_SUCCESS);
}
}
/* initialize push button */
if ((trigger_data.pb_pin) > 0) {
/* first free pin */
gpio_unexport(trigger_data.pb_pin);
gpio_export(trigger_data.pb_pin);
gpio_direction(trigger_data.pb_pin, 1);
gpio_edge(trigger_data.pb_pin, EDGE_FALLING);
trigger_data.pb_fd = gpio_open(trigger_data.pb_pin);
}
FD_ZERO(&readfds);
FD_ZERO(&exceptfds);
/* delete pending push button event */
if ((trigger_data.pb_pin) > 0) {
read(trigger_data.pb_fd, NULL, 100);
lseek(trigger_data.pb_fd, 0, SEEK_SET);
}
/* loop forever TODO: if interval is set */
while (1) {
FD_SET(trigger_data.socket, &readfds);
/* extend FD_SET only if push button pin is set */
if (trigger_data.pb_pin > 0)
FD_SET(trigger_data.pb_fd, &exceptfds);
if (select(MAX(trigger_data.socket, trigger_data.pb_fd) + 1, &readfds, NULL, &exceptfds, NULL) < 0) {
fprintf(stderr, "select error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* CAN frame event */
if (FD_ISSET(trigger_data.socket, &readfds)) {
if (read(trigger_data.socket, &frame, sizeof(struct can_frame)) < 0)
fprintf(stderr, "error reading CAN frame: %s\n", strerror(errno));
if (frame.can_id & CAN_EFF_FLAG) {
switch ((frame.can_id & 0x00FF0000UL) >> 16) {
case 0x31:
if (trigger_data.verbose)
print_can_frame(F_CAN_FORMAT_STRG, &frame);
memcpy(&member, frame.data, sizeof(member));
member = ntohs(member);
/* look for MS2 */
if ((member & 0xfff0) == 0x4d50)
get_ms2_dbsize(&trigger_data);
break;
case 0x41:
if (trigger_data.verbose)
print_can_frame(F_CAN_FORMAT_STRG, &frame);
break;
case 0x42:
get_data(&trigger_data, &frame);
/* if (trigger_data.verbose)
print_can_frame(F_CAN_FORMAT_STRG, &frame); */
break;
default:
if (trigger_data.verbose)
print_can_frame(F_CAN_FORMAT_STRG, &frame);
break;
}
}
}
/* push button event */
if (FD_ISSET(trigger_data.pb_fd, &exceptfds)) {
set_led_pattern(&trigger_data, LED_ST_HB_FAST);
/* wait some time for LED pattern change */
usec_sleep(1000 * 1000);
/* send CAN Member Ping */
frame.can_id = 0x00300300;
frame.can_dlc = 0;
memset(frame.data, 0, 8);
if (send_can_frame(trigger_data.socket, &frame, trigger_data.verbose) < 0)
fprintf(stderr, "can't send CAN Member Ping: %s\n", strerror(errno));
lseek(trigger_data.pb_fd, 0, SEEK_SET);
if (read(trigger_data.pb_fd, buffer, sizeof(buffer)) < 0)
fprintf(stderr, "error reading GPIO status: %s\n", strerror(errno));
printf("push button event\n");
}
}
/* TODO : wait until copy is done */
if ((trigger_data.pb_pin) > 0)
gpio_unexport(trigger_data.pb_pin);
if ((trigger_data.led_pin) > 0) {
gpio_unexport(trigger_data.led_pin);
pthread_join(pth, (void *)&trigger_data);
pthread_mutex_unlock(&lock);
}
return 0;
}
int main(int argc, char **argv) {
pid_t pid;
int max_fds, opt;
struct can_frame frame;
int sc;
struct sockaddr_can caddr;
struct ifreq ifr;
socklen_t caddrlen = sizeof(caddr);
fd_set read_fds;
int background = 0;
int verbose = 1;
strcpy(ifr.ifr_name, "vcan1");
while ((opt = getopt(argc, argv, "i:dh?")) != -1) {
switch (opt) {
case 'i':
strncpy(ifr.ifr_name, optarg, sizeof(ifr.ifr_name) - 1);
break;
case 'd':
verbose = 0;
background = 1;
break;
case 'h':
case '?':
print_usage(basename(argv[0]));
exit(EXIT_SUCCESS);
break;
default:
fprintf(stderr, "Unknown option %c\n", opt);
print_usage(basename(argv[0]));
exit(EXIT_FAILURE);
}
}
memset(&caddr, 0, sizeof(caddr));
/* prepare CAN socket */
if ((sc = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
fprintf(stderr, "error creating CAN socket: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_family = AF_CAN;
if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
fprintf(stderr, "setup CAN socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_ifindex = ifr.ifr_ifindex;
if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
fprintf(stderr, "error binding CAN socket: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* daemonize the process if requested */
if (background) {
/* fork off the parent process */
pid = fork();
if (pid < 0) {
exit(EXIT_FAILURE);
}
/* if we got a good PID, then we can exit the parent process */
if (pid > 0) {
printf("Going into background ...\n");
exit(EXIT_SUCCESS);
}
}
FD_ZERO(&read_fds);
FD_SET(sc, &read_fds);
max_fds = sc;
while (1) {
if (select(max_fds + 1, &read_fds, NULL, NULL, NULL) < 0) {
fprintf(stderr, "select error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* received a CAN frame */
if (FD_ISSET(sc, &read_fds)) {
if (read(sc, &frame, sizeof(struct can_frame)) < 0) {
fprintf(stderr, "error reading CAN frame: %s\n", strerror(errno));
} else if (frame.can_id & CAN_EFF_FLAG) { /* only EFF frames are valid */
if (verbose) {
print_can_frame(F_CAN_FORMAT_STRG, &frame);
}
switch ((frame.can_id & 0x00FF0000UL) >> 16) {
case 0x00:
if ((memcmp(&frame.data[0], &M_GLEISBOX_ID[5], 4) == 0) ||
(memcmp(&frame.data[0], M_ALL_ID, 4) == 0)) {
if (frame.data[4] == 0x0b) {
switch (frame.data[5]) {
case 0x01:
send_defined_can_frame(sc, M_GLEISBOX_VAL1, verbose);
break;
case 0x03:
send_defined_can_frame(sc, M_GLEISBOX_VAL3, verbose);
break;
case 0x04:
send_defined_can_frame(sc, M_GLEISBOX_VAL4, verbose);
break;
default:
break;
}
} else {
/* frame.can_id &= 0xFFFF0000UL; */
frame.can_id |= 0x00010000UL;
send_can_frame(sc, &frame, verbose);
}
}
break;
case 0x08: /* loco speed */
case 0x0A: /* loco direction */
case 0x0C: /* loco function */
case 0x16: /* extension */
/* frame.can_id &= 0xFFFF0000UL; */
frame.can_id |= 0x00010000UL;
send_can_frame(sc, &frame, verbose);
break;
case 0x30: /* ping / ID /software */
send_defined_can_frame(sc, M_GLEISBOX_ID, verbose);
break;
case 0x3A: /* status */
if (memcmp(&frame.data[0], &M_GLEISBOX_ID[5], 4) == 0) {
switch (frame.data[4]) {
case 0x00:
send_defined_can_frame(sc, M_GLEISBOX_STATUS1, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS2, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS3, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS4, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS5, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS6, verbose);
break;
case 0x01:
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_1, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_2, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_3, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_4, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_5, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_6, verbose);
break;
case 0x02:
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_1, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_2, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_3, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_4, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_5, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_6, verbose);
break;
case 0x03:
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_1, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_2, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_3, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_4, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_5, verbose);
break;
default:
break;
}
}
case 0x36: /* upgrade process) */
if (frame.can_dlc == 0)
send_defined_can_frame(sc, M_GLEISBOX_BL_INIT, verbose);
if ((frame.can_dlc == 6) && (frame.data[4] == 0x44)) {
frame.can_id = 0x00379B32UL;
send_can_frame(sc, &frame, verbose);
}
if ((frame.can_dlc == 7) && (frame.data[4] == 0x88)) {
frame.can_dlc = 5;
frame.can_id = 0x00379B32UL;
/* test case : crc fault */
/* frame.data[4] = 0xf2; */
send_can_frame(sc, &frame, verbose);
}
break;
default:
break;
}
} else
print_can_frame(F_S_CAN_FORMAT_STRG, &frame);
}
}
