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); } }