diff --git a/common/util.cc b/common/util.cc index 3d03c96bbe..26a2bd60bc 100644 --- a/common/util.cc +++ b/common/util.cc @@ -1,4 +1,5 @@ #include "common/util.h" +#include "common/swaglog.h" #include #include @@ -151,11 +152,16 @@ int safe_fflush(FILE *stream) { return ret; } -int safe_ioctl(int fd, unsigned long request, void *argp) { +int safe_ioctl(int fd, unsigned long request, void *argp, const char* exception_msg) { int ret; do { ret = ioctl(fd, request, argp); } while ((ret == -1) && (errno == EINTR)); + + if (ret == -1 && exception_msg) { + LOGE("safe_ioctl error: %s %s(%d) (fd: %d request: %lx argp: %p)", exception_msg, strerror(errno), errno, fd, request, argp); + throw std::runtime_error(exception_msg); + } return ret; } diff --git a/common/util.h b/common/util.h index 4fb4b13fae..f46db4d9fa 100644 --- a/common/util.h +++ b/common/util.h @@ -88,7 +88,7 @@ int write_file(const char* path, const void* data, size_t size, int flags = O_WR FILE* safe_fopen(const char* filename, const char* mode); size_t safe_fwrite(const void * ptr, size_t size, size_t count, FILE * stream); int safe_fflush(FILE *stream); -int safe_ioctl(int fd, unsigned long request, void *argp); +int safe_ioctl(int fd, unsigned long request, void *argp, const char* exception_msg = nullptr); std::string readlink(const std::string& path); bool file_exists(const std::string& fn); diff --git a/selfdrive/pandad/spi.cc b/selfdrive/pandad/spi.cc index 108b11b9dc..b6ee57801a 100644 --- a/selfdrive/pandad/spi.cc +++ b/selfdrive/pandad/spi.cc @@ -66,58 +66,44 @@ PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { // 50MHz is the max of the 845. note that some older // revs of the comma three may not support this speed uint32_t spi_speed = 50000000; + try { + if (!util::file_exists(SPI_DEVICE)) { + throw std::runtime_error("Error connecting to panda: SPI device not found"); + } - if (!util::file_exists(SPI_DEVICE)) { - goto fail; - } - - spi_fd = open(SPI_DEVICE.c_str(), O_RDWR); - if (spi_fd < 0) { - LOGE("failed opening SPI device %d", spi_fd); - goto fail; - } - - // SPI settings - ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode); - if (ret < 0) { - LOGE("failed setting SPI mode %d", ret); - goto fail; - } - - ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - if (ret < 0) { - LOGE("failed setting SPI speed"); - goto fail; - } + spi_fd = open(SPI_DEVICE.c_str(), O_RDWR); + if (spi_fd < 0) { + LOGE("failed opening SPI device %d", spi_fd); + throw std::runtime_error("Error connecting to panda: failed to open SPI device"); + } - ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word); - if (ret < 0) { - LOGE("failed setting SPI bits per word"); - goto fail; - } + // SPI settings + util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode, "failed setting SPI mode"); + util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed, "failed setting SPI speed"); + util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word, "failed setting SPI bits per word"); + + // get hw UID/serial + ret = control_read(0xc3, 0, 0, uid, uid_len, 100); + if (ret == uid_len) { + std::stringstream stream; + for (int i = 0; i < uid_len; i++) { + stream << std::hex << std::setw(2) << std::setfill('0') << int(uid[i]); + } + hw_serial = stream.str(); + } else { + LOGD("failed to get serial %d", ret); + throw std::runtime_error("Error connecting to panda: failed to get serial"); + } - // get hw UID/serial - ret = control_read(0xc3, 0, 0, uid, uid_len, 100); - if (ret == uid_len) { - std::stringstream stream; - for (int i = 0; i < uid_len; i++) { - stream << std::hex << std::setw(2) << std::setfill('0') << int(uid[i]); + if (!serial.empty() && (serial != hw_serial)) { + throw std::runtime_error("Error connecting to panda: serial mismatch"); } - hw_serial = stream.str(); - } else { - LOGD("failed to get serial %d", ret); - goto fail; - } - if (!serial.empty() && (serial != hw_serial)) { - goto fail; + } catch (...) { + cleanup(); + throw; } - return; - -fail: - cleanup(); - throw std::runtime_error("Error connecting to panda"); } PandaSpiHandle::~PandaSpiHandle() { diff --git a/system/loggerd/encoder/v4l_encoder.cc b/system/loggerd/encoder/v4l_encoder.cc index ac1da09693..e4b4c8a093 100644 --- a/system/loggerd/encoder/v4l_encoder.cc +++ b/system/loggerd/encoder/v4l_encoder.cc @@ -24,14 +24,6 @@ */ const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0; -static void checked_ioctl(int fd, unsigned long request, void *argp) { - int ret = util::safe_ioctl(fd, request, argp); - if (ret != 0) { - LOGE("checked_ioctl failed with error %d (%d %lx %p)", errno, fd, request, argp); - assert(0); - } -} - static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=NULL, unsigned int *bytesused=NULL, unsigned int *flags=NULL, struct timeval *timestamp=NULL) { v4l2_plane plane = {0}; v4l2_buffer v4l_buf = { @@ -40,7 +32,7 @@ static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=N .m = { .planes = &plane, }, .length = 1, }; - checked_ioctl(fd, VIDIOC_DQBUF, &v4l_buf); + util::safe_ioctl(fd, VIDIOC_DQBUF, &v4l_buf, "VIDIOC_DQBUF failed"); if (index) *index = v4l_buf.index; if (bytesused) *bytesused = v4l_buf.m.planes[0].bytesused; @@ -66,8 +58,7 @@ static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, Vis .flags = V4L2_BUF_FLAG_TIMESTAMP_COPY, .timestamp = timestamp }; - - checked_ioctl(fd, VIDIOC_QBUF, &v4l_buf); + util::safe_ioctl(fd, VIDIOC_QBUF, &v4l_buf, "VIDIOC_QBUF failed"); } static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) { @@ -76,7 +67,7 @@ static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) .memory = V4L2_MEMORY_USERPTR, .count = count }; - checked_ioctl(fd, VIDIOC_REQBUFS, &reqbuf); + util::safe_ioctl(fd, VIDIOC_REQBUFS, &reqbuf, "VIDIOC_REQBUFS failed"); } void V4LEncoder::dequeue_handler(V4LEncoder *e) { @@ -159,7 +150,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei assert(fd >= 0); struct v4l2_capability cap; - checked_ioctl(fd, VIDIOC_QUERYCAP, &cap); + util::safe_ioctl(fd, VIDIOC_QUERYCAP, &cap, "VIDIOC_QUERYCAP failed"); LOGD("opened encoder device %s %s = %d", cap.driver, cap.card, fd); assert(strcmp((const char *)cap.driver, "msm_vidc_driver") == 0); assert(strcmp((const char *)cap.card, "msm_vidc_venc") == 0); @@ -177,7 +168,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei } } }; - checked_ioctl(fd, VIDIOC_S_FMT, &fmt_out); + util::safe_ioctl(fd, VIDIOC_S_FMT, &fmt_out, "VIDIOC_S_FMT failed"); v4l2_streamparm streamparm = { .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, @@ -191,7 +182,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei } } }; - checked_ioctl(fd, VIDIOC_S_PARM, &streamparm); + util::safe_ioctl(fd, VIDIOC_S_PARM, &streamparm, "VIDIOC_S_PARM failed"); struct v4l2_format fmt_in = { .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, @@ -205,7 +196,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei } } }; - checked_ioctl(fd, VIDIOC_S_FMT, &fmt_in); + util::safe_ioctl(fd, VIDIOC_S_FMT, &fmt_in, "VIDIOC_S_FMT failed"); LOGD("in buffer size %d, out buffer size %d", fmt_in.fmt.pix_mp.plane_fmt[0].sizeimage, @@ -221,7 +212,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei { .id = V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD, .value = 1}, }; for (auto ctrl : ctrls) { - checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl); + util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL failed"); } } @@ -234,7 +225,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0}, }; for (auto ctrl : ctrls) { - checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl); + util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL failed"); } } else { struct v4l2_control ctrls[] = { @@ -250,7 +241,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei { .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE, .value = 0}, }; for (auto ctrl : ctrls) { - checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl); + util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL failed"); } } @@ -260,9 +251,9 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei // start encoder v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; - checked_ioctl(fd, VIDIOC_STREAMON, &buf_type); + util::safe_ioctl(fd, VIDIOC_STREAMON, &buf_type, "VIDIOC_STREAMON failed"); buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; - checked_ioctl(fd, VIDIOC_STREAMON, &buf_type); + util::safe_ioctl(fd, VIDIOC_STREAMON, &buf_type, "VIDIOC_STREAMON failed"); // queue up output buffers for (unsigned int i = 0; i < BUF_OUT_COUNT; i++) { @@ -305,7 +296,7 @@ void V4LEncoder::encoder_close() { for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.push(i); // no frames, stop the encoder struct v4l2_encoder_cmd encoder_cmd = { .cmd = V4L2_ENC_CMD_STOP }; - checked_ioctl(fd, VIDIOC_ENCODER_CMD, &encoder_cmd); + util::safe_ioctl(fd, VIDIOC_ENCODER_CMD, &encoder_cmd, "VIDIOC_ENCODER_CMD failed"); // join waits for V4L2_QCOM_BUF_FLAG_EOS dequeue_handler_thread.join(); assert(extras.empty()); @@ -316,10 +307,10 @@ void V4LEncoder::encoder_close() { V4LEncoder::~V4LEncoder() { encoder_close(); v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; - checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type); + util::safe_ioctl(fd, VIDIOC_STREAMOFF, &buf_type, "VIDIOC_STREAMOFF failed"); request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, 0); buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; - checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type); + util::safe_ioctl(fd, VIDIOC_STREAMOFF, &buf_type, "VIDIOC_STREAMOFF failed"); request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 0); close(fd);