void *PullThreadQueue(ThreadQueue *q, uint32_t timeout) { int err = SemaphoreWaitTimeout(q->sem, timeout); if (err == 0) { void *p = QueueFront(q->queue); QueuePopFront(q->queue); return p; } if (err != -ETIMEDOUT) error("failed: %d.", err); return NULL; }
Error I2cWriteRead(uint8_t a, void const * w, size_t wc, void * r, size_t rc) { assert(w || !wc); assert(r || !rc); assert(state == STATE_IDLE); addr = a; write = (uint8_t const *) w; write_count = wc; read = (uint8_t *) r; read_count = rc; result = false; _MI2C2IE = 1; _MI2C1IE = 1; // Assert a start condition to kick off the state machine. state = STATE_START; I2C2CONbits.SEN = 1; // Start. // Wait for completion. bool ret; Error e = QueuePopFront(result_queue, &ret); _MI2C2IE = 0; // Disable interrupts. _MI2C1IE = 0; _MI2C2IF = 0; // Clear any lingering interrupts. _MI2C1IF = 0; QueueReset(result_queue); // Clear any lingering results. state = STATE_IDLE; if (!I2C2CONbits.I2CEN) { // We had to shut-off the module because of a bus collision. Re-enable. I2C2CONbits.I2CEN = 1; } if (e != ERROR_NONE) return e; if (!ret) return ERROR_FAILURE; return ERROR_NONE; }