1 /* 2 * Copyright (c) 2026 Abdelkader Boudih <freebsd@seuros.com> 3 * 4 * SPDX-License-Identifier: BSD-2-Clause 5 */ 6 7 /* 8 * fwcam(4) - IIDC 1394-based Digital Camera driver 9 * 10 * Implements the IIDC v1.30 specification (TA Document 1999023) for 11 * FireWire digital cameras. 12 */ 13 14 #include <sys/param.h> 15 #include <sys/systm.h> 16 #include <sys/module.h> 17 #include <sys/bus.h> 18 #include <sys/kernel.h> 19 #include <sys/conf.h> 20 #include <sys/malloc.h> 21 #include <sys/lock.h> 22 #include <sys/mutex.h> 23 #include <sys/sysctl.h> 24 #include <sys/taskqueue.h> 25 #include <sys/fcntl.h> 26 #include <sys/poll.h> 27 #include <sys/selinfo.h> 28 #include <sys/uio.h> 29 #include <sys/mbuf.h> 30 31 #include <dev/firewire/firewire.h> 32 #include <dev/firewire/firewirereg.h> 33 #include <dev/firewire/iec13213.h> 34 #include <dev/firewire/fwcam.h> 35 #include <dev/firewire/fw_helpers.h> 36 37 static MALLOC_DEFINE(M_FWCAM, "fwcam", "IIDC FireWire Camera"); 38 39 static int debug = 0; 40 static int iso_channel = 0; 41 SYSCTL_DECL(_hw_firewire); 42 static SYSCTL_NODE(_hw_firewire, OID_AUTO, fwcam, CTLFLAG_RD | CTLFLAG_MPSAFE, 43 0, "IIDC Camera"); 44 SYSCTL_INT(_hw_firewire_fwcam, OID_AUTO, debug, CTLFLAG_RWTUN, &debug, 0, 45 "fwcam debug level"); 46 SYSCTL_INT(_hw_firewire_fwcam, OID_AUTO, iso_channel, CTLFLAG_RWTUN, 47 &iso_channel, 0, "ISO channel for isochronous receive (default 0)"); 48 49 #define FWCAM_DEBUG(lev, fmt, ...) \ 50 do { \ 51 if (debug >= (lev)) \ 52 printf("fwcam: " fmt, ## __VA_ARGS__); \ 53 } while (0) 54 55 static int fwcam_probe(device_t); 56 static int fwcam_attach(device_t); 57 static int fwcam_detach(device_t); 58 static void fwcam_probe_task(void *, int); 59 static int fwcam_iso_start(struct fwcam_softc *); 60 static void fwcam_iso_stop(struct fwcam_softc *); 61 static void fwcam_iso_input(struct fw_xferq *); 62 63 static d_open_t fwcam_cdev_open; 64 static d_close_t fwcam_cdev_close; 65 static d_read_t fwcam_cdev_read; 66 static d_poll_t fwcam_cdev_poll; 67 static d_ioctl_t fwcam_cdev_ioctl; 68 69 static struct cdevsw fwcam_cdevsw = { 70 .d_version = D_VERSION, 71 .d_flags = D_TRACKCLOSE, 72 .d_open = fwcam_cdev_open, 73 .d_close = fwcam_cdev_close, 74 .d_read = fwcam_cdev_read, 75 .d_poll = fwcam_cdev_poll, 76 .d_ioctl = fwcam_cdev_ioctl, 77 .d_name = "fwcam", 78 }; 79 80 /* 81 * Search a CSR directory for the IIDC command base register (key 0x40). 82 * The iSight places cmd_base inside a logical_unit_directory nested 83 * within the IIDC unit directory, so we recurse into sub-directories 84 * up to max_depth levels. 85 */ 86 static uint32_t 87 fwcam_search_dir_cmd_base(uint32_t *csrrom, uint32_t dir_qoff, 88 uint32_t rom_quads, int max_depth) 89 { 90 struct csrdirectory *dir; 91 struct csrreg *reg; 92 uint32_t dir_len, sub_qoff, val; 93 int i; 94 95 if (dir_qoff >= rom_quads || max_depth <= 0) 96 return (0); 97 dir = (struct csrdirectory *)&csrrom[dir_qoff]; 98 dir_len = dir->crc_len; 99 if (dir_qoff + 1 + dir_len > rom_quads) 100 return (0); 101 if (!crom_crc_valid((uint32_t *)&dir->entry[0], dir_len, dir->crc)) { 102 if (firewire_debug) 103 printf("fwcam: bad CRC in directory at 0x%x\n", 104 dir_qoff); 105 } 106 107 for (i = 0; i < (int)dir_len; i++) { 108 if (dir_qoff + 1 + i >= rom_quads) 109 break; 110 reg = &dir->entry[i]; 111 if (reg->key == IIDC_CROM_CMD_BASE && reg->val != 0) 112 return (reg->val); 113 if ((reg->key & CSRTYPE_MASK) == CSRTYPE_D) { 114 sub_qoff = dir_qoff + 1 + i + reg->val; 115 val = fwcam_search_dir_cmd_base(csrrom, sub_qoff, 116 rom_quads, max_depth - 1); 117 if (val != 0) 118 return (val); 119 } 120 } 121 return (0); 122 } 123 124 /* 125 * Extract IIDC command base register from the unit directory. 126 * Searches the unit directory and any sub-directories (the iSight 127 * places cmd_base inside a logical_unit_directory). 128 */ 129 static uint32_t 130 fwcam_find_iidc_cmd_base(struct fw_unit *unit) 131 { 132 133 return (fwcam_search_dir_cmd_base(unit->fwdev->csrrom, 134 unit->dir_offset / 4, CSRROMSIZE / 4, 2)); 135 } 136 137 static int 138 fwcam_read_quadlet(struct fwcam_softc *sc, uint32_t offset, uint32_t *val) 139 { 140 uint16_t dst; 141 uint8_t spd; 142 int err; 143 144 FWCAM_LOCK(sc); 145 if (sc->fwdev == NULL) { 146 FWCAM_UNLOCK(sc); 147 return (ENXIO); 148 } 149 dst = FWLOCALBUS | sc->fwdev->dst; 150 spd = min(sc->fwdev->speed, FWSPD_S400); 151 FWCAM_UNLOCK(sc); 152 153 err = fw_read_quadlet(sc->fd.fc, M_FWCAM, dst, spd, 154 sc->cmd_hi, sc->cmd_lo + offset, val); 155 if (err) 156 FWCAM_DEBUG(1, "read_quadlet: offset=0x%x err=%d\n", 157 offset, err); 158 return (err); 159 } 160 161 static int 162 fwcam_write_quadlet(struct fwcam_softc *sc, uint32_t offset, uint32_t val) 163 { 164 uint16_t dst; 165 uint8_t spd; 166 int err; 167 168 FWCAM_LOCK(sc); 169 if (sc->fwdev == NULL) { 170 FWCAM_UNLOCK(sc); 171 return (ENXIO); 172 } 173 dst = FWLOCALBUS | sc->fwdev->dst; 174 spd = min(sc->fwdev->speed, FWSPD_S400); 175 FWCAM_UNLOCK(sc); 176 177 err = fw_write_quadlet(sc->fd.fc, M_FWCAM, dst, spd, 178 sc->cmd_hi, sc->cmd_lo + offset, val); 179 if (err) 180 FWCAM_DEBUG(1, "write_quadlet: offset=0x%x err=%d\n", 181 offset, err); 182 return (err); 183 } 184 185 static int 186 fwcam_read_capabilities(struct fwcam_softc *sc) 187 { 188 int err, f, m; 189 190 err = fwcam_read_quadlet(sc, IIDC_V_FORMAT_INQ, &sc->formats); 191 if (err) { 192 device_printf(sc->fd.dev, 193 "failed to read V_FORMAT_INQ: %d\n", err); 194 return (err); 195 } 196 197 err = fwcam_read_quadlet(sc, IIDC_BASIC_FUNC_INQ, &sc->basic_func); 198 if (err) { 199 device_printf(sc->fd.dev, 200 "failed to read BASIC_FUNC_INQ: %d\n", err); 201 return (err); 202 } 203 204 /* Read mode and rate inquiry registers for each supported format */ 205 for (f = 0; f < 8; f++) { 206 if (!(sc->formats & (1 << (31 - f)))) { 207 sc->modes[f] = 0; 208 continue; 209 } 210 err = fwcam_read_quadlet(sc, IIDC_V_MODE_INQ(f), 211 &sc->modes[f]); 212 if (err) { 213 sc->modes[f] = 0; 214 continue; 215 } 216 for (m = 0; m < 8; m++) { 217 if (!(sc->modes[f] & (1 << (31 - m)))) { 218 sc->rates[f][m] = 0; 219 continue; 220 } 221 err = fwcam_read_quadlet(sc, IIDC_V_RATE_INQ(f, m), 222 &sc->rates[f][m]); 223 if (err) 224 sc->rates[f][m] = 0; 225 } 226 } 227 228 err = fwcam_read_quadlet(sc, IIDC_FEATURE_HI_INQ, &sc->features_hi); 229 if (err) 230 sc->features_hi = 0; 231 232 err = fwcam_read_quadlet(sc, IIDC_FEATURE_LO_INQ, &sc->features_lo); 233 if (err) 234 sc->features_lo = 0; 235 236 return (0); 237 } 238 239 static int 240 fwcam_power_on(struct fwcam_softc *sc) 241 { 242 uint32_t val; 243 int err, retries; 244 245 err = fwcam_read_quadlet(sc, IIDC_BASIC_FUNC_INQ, &val); 246 if (err) { 247 device_printf(sc->fd.dev, 248 "cannot read BASIC_FUNC_INQ: %d\n", err); 249 return (err); 250 } 251 252 if ((val & IIDC_CAM_POWER_CTRL) == 0) { 253 FWCAM_DEBUG(1, "no power control, assuming powered\n"); 254 return (0); 255 } 256 257 err = fwcam_read_quadlet(sc, IIDC_CAMERA_POWER, &val); 258 if (err == 0 && (val & IIDC_POWER_ON)) { 259 FWCAM_DEBUG(1, "camera already powered on\n"); 260 return (0); 261 } 262 263 err = fwcam_write_quadlet(sc, IIDC_CAMERA_POWER, IIDC_POWER_ON); 264 if (err) { 265 device_printf(sc->fd.dev, 266 "failed to write CAMERA_POWER: %d\n", err); 267 return (err); 268 } 269 270 for (retries = 0; retries < 50; retries++) { 271 pause("fwcampw", hz / 10); 272 273 if (sc->state == FWCAM_STATE_DETACHING) 274 return (ENXIO); 275 276 err = fwcam_read_quadlet(sc, IIDC_CAMERA_POWER, &val); 277 if (err) 278 continue; /* read may fail while powering up */ 279 280 if (val & IIDC_POWER_ON) 281 return (0); 282 } 283 284 device_printf(sc->fd.dev, "camera power-on timeout (5s)\n"); 285 return (ETIMEDOUT); 286 } 287 288 static void 289 fwcam_probe_task(void *arg, int pending __unused) 290 { 291 struct fwcam_softc *sc = (struct fwcam_softc *)arg; 292 int err; 293 294 if (sc->state == FWCAM_STATE_DETACHING || sc->fwdev == NULL) 295 return; 296 297 err = fwcam_power_on(sc); 298 if (err) { 299 device_printf(sc->fd.dev, 300 "power-on failed (%d), trying to read anyway\n", err); 301 } 302 303 if (sc->state == FWCAM_STATE_DETACHING) 304 return; 305 306 if (fwcam_read_capabilities(sc) == 0) { 307 uint32_t val; 308 309 if (fwcam_read_quadlet(sc, IIDC_CUR_V_FORMAT, &val) == 0) 310 sc->cur_format = (val >> 29) & 0x7; 311 if (fwcam_read_quadlet(sc, IIDC_CUR_V_MODE, &val) == 0) 312 sc->cur_mode = (val >> 29) & 0x7; 313 if (fwcam_read_quadlet(sc, IIDC_CUR_V_FRM_RATE, &val) == 0) 314 sc->cur_framerate = (val >> 29) & 0x7; 315 316 FWCAM_LOCK(sc); 317 if (sc->state == FWCAM_STATE_DETACHING) { 318 FWCAM_UNLOCK(sc); 319 return; 320 } 321 sc->state = FWCAM_STATE_PROBED; 322 wakeup(sc); 323 FWCAM_UNLOCK(sc); 324 325 if (sc->dma_ch >= 0 && 326 sc->state != FWCAM_STATE_DETACHING) 327 fwcam_iso_start(sc); 328 } else { 329 FWCAM_LOCK(sc); 330 if (sc->state == FWCAM_STATE_PROBING) 331 sc->state = FWCAM_STATE_IDLE; 332 wakeup(sc); 333 FWCAM_UNLOCK(sc); 334 } 335 } 336 337 /* 338 * Compute expected frame size for current format/mode. 339 * Format_0 (VGA) modes: 340 * Mode 0: 160x120 YUV444 = 160*120*3 = 57600 341 * Mode 1: 320x240 YUV422 = 320*240*2 = 153600 342 * Mode 2: 640x480 YUV411 = 640*480*3/2 = 460800 343 * Mode 3: 640x480 YUV422 = 640*480*2 = 614400 344 * Mode 4: 640x480 RGB8 = 640*480*3 = 921600 345 * Mode 5: 640x480 Mono8 = 640*480 = 307200 346 */ 347 static uint32_t 348 fwcam_frame_size(struct fwcam_softc *sc) 349 { 350 static const uint32_t fmt0_sizes[] = { 351 160 * 120 * 3, /* mode 0: YUV444 */ 352 320 * 240 * 2, /* mode 1: YUV422 */ 353 640 * 480 * 3 / 2, /* mode 2: YUV411 */ 354 640 * 480 * 2, /* mode 3: YUV422 */ 355 640 * 480 * 3, /* mode 4: RGB8 */ 356 640 * 480, /* mode 5: Mono8 */ 357 }; 358 359 if (sc->cur_format == IIDC_FMT_VGA && sc->cur_mode < nitems(fmt0_sizes)) 360 return (fmt0_sizes[sc->cur_mode]); 361 362 /* Default to largest VGA mode */ 363 return (FWCAM_MAX_FRAME_SIZE); 364 } 365 366 static int 367 fwcam_iso_start(struct fwcam_softc *sc) 368 { 369 struct firewire_comm *fc = sc->fd.fc; 370 struct fw_xferq *xferq; 371 uint32_t val; 372 int dma_ch, err; 373 374 mtx_assert(&sc->mtx, MA_NOTOWNED); 375 376 FWCAM_LOCK(sc); 377 if (sc->dma_ch >= 0 || sc->state == FWCAM_STATE_STREAMING) { 378 FWCAM_UNLOCK(sc); 379 return (0); /* already running or starting */ 380 } 381 if (sc->state == FWCAM_STATE_DETACHING) { 382 FWCAM_UNLOCK(sc); 383 return (ENXIO); 384 } 385 FWCAM_UNLOCK(sc); 386 387 dma_ch = fw_open_isodma(fc, 0); 388 if (dma_ch < 0) { 389 device_printf(sc->fd.dev, "no IR DMA channel available\n"); 390 return (EBUSY); 391 } 392 393 FWCAM_LOCK(sc); 394 if (sc->dma_ch >= 0) { 395 FWCAM_UNLOCK(sc); 396 fc->ir[dma_ch]->flag &= ~FWXFERQ_OPEN; 397 return (0); 398 } 399 FWCAM_UNLOCK(sc); 400 401 xferq = fc->ir[dma_ch]; 402 xferq->flag |= FWXFERQ_EXTBUF | FWXFERQ_HANDLER | FWXFERQ_STREAM; 403 404 sc->iso_channel = (uint8_t)(iso_channel & FWXFERQ_CHTAGMASK); 405 xferq->flag &= ~FWXFERQ_CHTAGMASK; 406 xferq->flag |= sc->iso_channel & FWXFERQ_CHTAGMASK; 407 408 xferq->sc = (caddr_t)sc; 409 xferq->hand = fwcam_iso_input; 410 xferq->bnchunk = FWCAM_ISO_NCHUNK; 411 xferq->bnpacket = 1; 412 xferq->psize = FWCAM_ISO_PKTSIZE; 413 xferq->queued = 0; 414 xferq->buf = NULL; 415 416 xferq->bulkxfer = malloc(sizeof(struct fw_bulkxfer) * xferq->bnchunk, 417 M_FWCAM, M_WAITOK | M_ZERO); 418 419 fw_iso_init_chunks(xferq); 420 421 sc->frame_size = fwcam_frame_size(sc); 422 sc->frame_buf = malloc(sc->frame_size, M_FWCAM, M_WAITOK | M_ZERO); 423 sc->read_buf = malloc(sc->frame_size, M_FWCAM, M_WAITOK | M_ZERO); 424 sc->frame_offset = 0; 425 sc->frame_ready = 0; 426 sc->frame_dropped = 0; 427 428 /* IIDC spec s3.1: set video mode registers before ISO enable */ 429 err = fwcam_write_quadlet(sc, IIDC_CUR_V_FORMAT, 430 (uint32_t)sc->cur_format << IIDC_CUR_V_SHIFT); 431 if (err) { 432 device_printf(sc->fd.dev, 433 "failed to set CUR_V_FORMAT: %d\n", err); 434 goto fail; 435 } 436 err = fwcam_write_quadlet(sc, IIDC_CUR_V_MODE, 437 (uint32_t)sc->cur_mode << IIDC_CUR_V_SHIFT); 438 if (err) { 439 device_printf(sc->fd.dev, 440 "failed to set CUR_V_MODE: %d\n", err); 441 goto fail; 442 } 443 err = fwcam_write_quadlet(sc, IIDC_CUR_V_FRM_RATE, 444 (uint32_t)sc->cur_framerate << IIDC_CUR_V_SHIFT); 445 if (err) { 446 device_printf(sc->fd.dev, 447 "failed to set CUR_V_FRM_RATE: %d\n", err); 448 goto fail; 449 } 450 451 /* Check Vmode_Error_Status - camera rejects ISO_EN on error */ 452 err = fwcam_read_quadlet(sc, IIDC_VMODE_ERR_STATUS, &val); 453 if (err == 0 && (val & IIDC_VMODE_ERROR)) { 454 device_printf(sc->fd.dev, 455 "Vmode_Error_Status set: format=%d mode=%d rate=%d " 456 "speed=%d\n", sc->cur_format, sc->cur_mode, 457 sc->cur_framerate, sc->iso_speed); 458 err = EINVAL; 459 goto fail; 460 } 461 462 val = ((uint32_t)sc->iso_channel << IIDC_ISO_CH_SHIFT) | 463 ((uint32_t)sc->iso_speed << IIDC_ISO_SPEED_SHIFT); 464 err = fwcam_write_quadlet(sc, IIDC_ISO_CHANNEL, val); 465 if (err) { 466 device_printf(sc->fd.dev, 467 "failed to set ISO_CHANNEL: %d\n", err); 468 goto fail; 469 } 470 471 err = fwcam_write_quadlet(sc, IIDC_ISO_EN, IIDC_ISO_EN_ON); 472 if (err == EIO) { 473 /* 474 * Cameras with a lens cover might power down the sensor 475 * when the cover is closed and reject streaming requests. 476 * Try to re-power and retry once before giving up. 477 */ 478 err = fwcam_power_on(sc); 479 if (err == 0) 480 err = fwcam_write_quadlet(sc, IIDC_ISO_EN, 481 IIDC_ISO_EN_ON); 482 if (err) { 483 device_printf(sc->fd.dev, 484 "ISO enable refused (lens cover closed?)\n"); 485 goto fail; 486 } 487 } else if (err) { 488 device_printf(sc->fd.dev, 489 "failed to enable ISO: %d\n", err); 490 goto fail; 491 } 492 493 err = fc->irx_enable(fc, dma_ch); 494 if (err) { 495 device_printf(sc->fd.dev, 496 "failed to start IR DMA: %d\n", err); 497 fwcam_write_quadlet(sc, IIDC_ISO_EN, 0); 498 goto fail; 499 } 500 501 FWCAM_LOCK(sc); 502 if (sc->state == FWCAM_STATE_DETACHING) { 503 FWCAM_UNLOCK(sc); 504 fc->irx_disable(fc, dma_ch); 505 fwcam_write_quadlet(sc, IIDC_ISO_EN, 0); 506 err = ENXIO; 507 goto fail; 508 } 509 sc->dma_ch = dma_ch; 510 sc->state = FWCAM_STATE_STREAMING; 511 FWCAM_UNLOCK(sc); 512 513 return (0); 514 515 fail: 516 fw_iso_free_chunks(xferq, M_FWCAM); 517 xferq->flag &= ~(FWXFERQ_MODEMASK | FWXFERQ_OPEN | FWXFERQ_STREAM | 518 FWXFERQ_EXTBUF | FWXFERQ_HANDLER | FWXFERQ_CHTAGMASK); 519 xferq->hand = NULL; 520 521 free(sc->frame_buf, M_FWCAM); 522 free(sc->read_buf, M_FWCAM); 523 sc->frame_buf = NULL; 524 sc->read_buf = NULL; 525 sc->dma_ch = -1; 526 527 return (err); 528 } 529 530 static void 531 fwcam_iso_stop(struct fwcam_softc *sc) 532 { 533 struct firewire_comm *fc = sc->fd.fc; 534 struct fw_xferq *xferq; 535 int dma_ch; 536 537 FWCAM_LOCK(sc); 538 dma_ch = sc->dma_ch; 539 if (dma_ch < 0) { 540 FWCAM_UNLOCK(sc); 541 return; 542 } 543 sc->dma_ch = -1; /* claim ownership of teardown */ 544 FWCAM_UNLOCK(sc); 545 546 xferq = fc->ir[dma_ch]; 547 548 if (xferq->flag & FWXFERQ_RUNNING) 549 fc->irx_disable(fc, dma_ch); 550 551 if (sc->fwdev != NULL) 552 fwcam_write_quadlet(sc, IIDC_ISO_EN, 0); 553 554 FWCAM_LOCK(sc); 555 fw_iso_wait_inactive_locked(&sc->mtx, &sc->iso_active, "fwcamis"); 556 sc->frame_ready = 0; 557 while (sc->read_in_progress) 558 msleep(&sc->read_in_progress, &sc->mtx, PWAIT, "fwcamst", hz); 559 FWCAM_UNLOCK(sc); 560 561 xferq->flag &= ~(FWXFERQ_MODEMASK | FWXFERQ_OPEN | FWXFERQ_STREAM | 562 FWXFERQ_EXTBUF | FWXFERQ_HANDLER | FWXFERQ_CHTAGMASK); 563 xferq->hand = NULL; 564 565 fw_iso_free_chunks(xferq, M_FWCAM); 566 567 free(sc->frame_buf, M_FWCAM); 568 free(sc->read_buf, M_FWCAM); 569 sc->frame_buf = NULL; 570 sc->read_buf = NULL; 571 } 572 573 static void 574 fwcam_iso_input(struct fw_xferq *xferq) 575 { 576 struct fwcam_softc *sc = (struct fwcam_softc *)xferq->sc; 577 struct fw_bulkxfer *sxfer; 578 struct fw_pkt *fp; 579 struct mbuf *m; 580 uint8_t *payload; 581 uint32_t plen; 582 uint8_t *tmp; 583 int dma_ch; 584 585 FWCAM_LOCK(sc); 586 dma_ch = sc->dma_ch; 587 if (dma_ch < 0 || sc->frame_buf == NULL) { 588 FWCAM_UNLOCK(sc); 589 return; 590 } 591 sc->iso_active = 1; 592 FWCAM_UNLOCK(sc); 593 594 while ((sxfer = STAILQ_FIRST(&xferq->stvalid)) != NULL) { 595 STAILQ_REMOVE_HEAD(&xferq->stvalid, link); 596 597 m = fw_iso_dequeue(xferq, sxfer, sc->fd.fc); 598 if (m == NULL) 599 continue; 600 601 fp = mtod(m, struct fw_pkt *); 602 plen = fp->mode.stream.len; 603 if (plen == 0) { 604 m_freem(m); 605 continue; /* empty packet (padding) */ 606 } 607 608 if (fp->mode.stream.sy == 1) { 609 if (sc->frame_offset > 0) { 610 if (sc->frame_offset == sc->frame_size) { 611 FWCAM_LOCK(sc); 612 if (sc->read_in_progress) { 613 sc->frame_dropped++; 614 } else { 615 if (sc->frame_ready) 616 sc->frame_dropped++; 617 tmp = sc->read_buf; 618 sc->read_buf = sc->frame_buf; 619 sc->frame_buf = tmp; 620 sc->frame_ready = 1; 621 wakeup(sc); 622 selwakeup(&sc->rsel); 623 } 624 FWCAM_UNLOCK(sc); 625 } else { 626 sc->frame_dropped++; 627 } 628 } 629 sc->frame_offset = 0; 630 } 631 632 if (m->m_len < 4) { 633 m_freem(m); 634 continue; 635 } 636 payload = mtod(m, uint8_t *) + 4; 637 if (plen > (uint32_t)(m->m_len - 4)) 638 plen = m->m_len - 4; 639 640 if (sc->frame_offset + plen <= sc->frame_size) { 641 memcpy(sc->frame_buf + sc->frame_offset, payload, plen); 642 sc->frame_offset += plen; 643 } else { 644 sc->frame_dropped++; 645 sc->frame_offset = 0; 646 } 647 648 m_freem(m); 649 } 650 651 fw_iso_rearm_done(xferq, sc->fd.fc, &sc->mtx, &sc->iso_active, 652 &sc->dma_ch, dma_ch); 653 } 654 655 static int 656 fwcam_cdev_open(struct cdev *dev, int oflags, int devtype, struct thread *td) 657 { 658 struct fwcam_softc *sc = dev->si_drv1; 659 int err; 660 661 FWCAM_LOCK(sc); 662 if (sc->state == FWCAM_STATE_DETACHING) { 663 FWCAM_UNLOCK(sc); 664 return (ENXIO); 665 } 666 667 if (sc->state == FWCAM_STATE_IDLE) { 668 sc->state = FWCAM_STATE_PROBING; 669 FWCAM_UNLOCK(sc); 670 taskqueue_enqueue(taskqueue_thread, &sc->probe_task); 671 FWCAM_LOCK(sc); 672 } 673 674 while (sc->state == FWCAM_STATE_PROBING) { 675 err = msleep(sc, &sc->mtx, PCATCH, "fwcampr", 10 * hz); 676 if (err) { 677 FWCAM_UNLOCK(sc); 678 return (err == EWOULDBLOCK ? ETIMEDOUT : err); 679 } 680 } 681 682 if (sc->state != FWCAM_STATE_PROBED && 683 sc->state != FWCAM_STATE_STREAMING) { 684 FWCAM_UNLOCK(sc); 685 return (ENXIO); 686 } 687 688 sc->open_count++; 689 FWCAM_UNLOCK(sc); 690 return (0); 691 } 692 693 static int 694 fwcam_cdev_close(struct cdev *dev, int fflag, int devtype, struct thread *td) 695 { 696 struct fwcam_softc *sc = dev->si_drv1; 697 698 FWCAM_LOCK(sc); 699 sc->open_count--; 700 if (sc->open_count <= 0) { 701 sc->open_count = 0; 702 if (sc->state == FWCAM_STATE_STREAMING) { 703 FWCAM_UNLOCK(sc); 704 fwcam_iso_stop(sc); 705 FWCAM_LOCK(sc); 706 if (sc->state != FWCAM_STATE_DETACHING) 707 sc->state = FWCAM_STATE_PROBED; 708 } 709 } 710 FWCAM_UNLOCK(sc); 711 return (0); 712 } 713 714 static int 715 fwcam_cdev_read(struct cdev *dev, struct uio *uio, int ioflag) 716 { 717 struct fwcam_softc *sc = dev->si_drv1; 718 int err; 719 720 FWCAM_LOCK(sc); 721 if (sc->state == FWCAM_STATE_PROBED) { 722 FWCAM_UNLOCK(sc); 723 err = fwcam_iso_start(sc); 724 if (err) 725 return (err); 726 FWCAM_LOCK(sc); 727 } 728 while (!sc->frame_ready) { 729 if (sc->state != FWCAM_STATE_STREAMING) { 730 FWCAM_UNLOCK(sc); 731 return (ENXIO); 732 } 733 if (ioflag & FNONBLOCK) { 734 FWCAM_UNLOCK(sc); 735 return (EAGAIN); 736 } 737 err = msleep(sc, &sc->mtx, PCATCH, "fwcamrd", 5 * hz); 738 if (err) { 739 FWCAM_UNLOCK(sc); 740 return (err); 741 } 742 } 743 744 sc->frame_ready = 0; 745 if (sc->read_buf == NULL) { 746 FWCAM_UNLOCK(sc); 747 return (ENXIO); 748 } 749 sc->read_in_progress = 1; 750 FWCAM_UNLOCK(sc); 751 752 err = uiomove(sc->read_buf, 753 MIN(uio->uio_resid, sc->frame_size), uio); 754 755 FWCAM_LOCK(sc); 756 sc->read_in_progress = 0; 757 wakeup(&sc->read_in_progress); 758 FWCAM_UNLOCK(sc); 759 760 return (err); 761 } 762 763 static int 764 fwcam_cdev_poll(struct cdev *dev, int events, struct thread *td) 765 { 766 struct fwcam_softc *sc = dev->si_drv1; 767 int revents = 0; 768 769 FWCAM_LOCK(sc); 770 if (events & (POLLIN | POLLRDNORM)) { 771 if (sc->frame_ready) 772 revents |= events & (POLLIN | POLLRDNORM); 773 else 774 selrecord(td, &sc->rsel); 775 } 776 FWCAM_UNLOCK(sc); 777 return (revents); 778 } 779 780 static const uint32_t fwcam_feat_inq[] = { 781 [FWCAM_FEAT_BRIGHTNESS] = IIDC_BRIGHTNESS_INQ, 782 [FWCAM_FEAT_AUTO_EXPOSURE] = IIDC_AUTO_EXPOSURE_INQ, 783 [FWCAM_FEAT_SHARPNESS] = IIDC_SHARPNESS_INQ, 784 [FWCAM_FEAT_WHITE_BALANCE] = IIDC_WHITE_BAL_INQ, 785 [FWCAM_FEAT_HUE] = IIDC_HUE_INQ, 786 [FWCAM_FEAT_SATURATION] = IIDC_SATURATION_INQ, 787 [FWCAM_FEAT_GAMMA] = IIDC_GAMMA_INQ, 788 [FWCAM_FEAT_SHUTTER] = IIDC_SHUTTER_INQ, 789 [FWCAM_FEAT_GAIN] = IIDC_GAIN_INQ, 790 [FWCAM_FEAT_IRIS] = IIDC_IRIS_INQ, 791 [FWCAM_FEAT_FOCUS] = IIDC_FOCUS_INQ, 792 [FWCAM_FEAT_TEMPERATURE] = IIDC_TEMPERATURE_INQ, 793 [FWCAM_FEAT_TRIGGER] = IIDC_TRIGGER_INQ, 794 }; 795 796 static const uint32_t fwcam_feat_ctrl[] = { 797 [FWCAM_FEAT_BRIGHTNESS] = IIDC_BRIGHTNESS, 798 [FWCAM_FEAT_AUTO_EXPOSURE] = IIDC_AUTO_EXPOSURE, 799 [FWCAM_FEAT_SHARPNESS] = IIDC_SHARPNESS, 800 [FWCAM_FEAT_WHITE_BALANCE] = IIDC_WHITE_BALANCE, 801 [FWCAM_FEAT_HUE] = IIDC_HUE, 802 [FWCAM_FEAT_SATURATION] = IIDC_SATURATION, 803 [FWCAM_FEAT_GAMMA] = IIDC_GAMMA, 804 [FWCAM_FEAT_SHUTTER] = IIDC_SHUTTER, 805 [FWCAM_FEAT_GAIN] = IIDC_GAIN, 806 [FWCAM_FEAT_IRIS] = IIDC_IRIS, 807 [FWCAM_FEAT_FOCUS] = IIDC_FOCUS, 808 [FWCAM_FEAT_TEMPERATURE] = IIDC_TEMPERATURE, 809 [FWCAM_FEAT_TRIGGER] = IIDC_TRIGGER_MODE, 810 [FWCAM_FEAT_ZOOM] = IIDC_ZOOM, 811 [FWCAM_FEAT_PAN] = IIDC_PAN, 812 [FWCAM_FEAT_TILT] = IIDC_TILT, 813 }; 814 815 static int 816 fwcam_get_feature(struct fwcam_softc *sc, struct fwcam_feature *feat) 817 { 818 uint32_t inq, ctrl; 819 int err; 820 821 if (feat->id >= FWCAM_FEAT_MAX) 822 return (EINVAL); 823 824 feat->flags = 0; 825 feat->min = 0; 826 feat->max = 0; 827 if (feat->id < nitems(fwcam_feat_inq) && 828 fwcam_feat_inq[feat->id] != 0) { 829 err = fwcam_read_quadlet(sc, fwcam_feat_inq[feat->id], &inq); 830 if (err) 831 return (err); 832 833 if (inq & (1 << 31)) 834 feat->flags |= FWCAM_FEATF_PRESENT; 835 if (inq & (1 << 28)) 836 feat->flags |= FWCAM_FEATF_ONOFF; 837 if (inq & (1 << 27)) 838 feat->flags |= FWCAM_FEATF_AUTO; 839 if (inq & (1 << 26)) 840 feat->flags |= FWCAM_FEATF_MANUAL; 841 feat->min = (inq >> 12) & 0xfff; 842 feat->max = inq & 0xfff; 843 } 844 845 if (feat->id >= nitems(fwcam_feat_ctrl) || 846 fwcam_feat_ctrl[feat->id] == 0) 847 return (EINVAL); 848 849 err = fwcam_read_quadlet(sc, fwcam_feat_ctrl[feat->id], &ctrl); 850 if (err) 851 return (err); 852 853 feat->value = ctrl & 0xfff; 854 /* White balance has U/B in bits [20:31] and V/R in bits [8:19] */ 855 if (feat->id == FWCAM_FEAT_WHITE_BALANCE) 856 feat->value2 = (ctrl >> 12) & 0xfff; 857 else 858 feat->value2 = 0; 859 860 return (0); 861 } 862 863 static int 864 fwcam_set_feature(struct fwcam_softc *sc, struct fwcam_feature *feat) 865 { 866 uint32_t ctrl, val; 867 int err; 868 869 if (feat->id >= FWCAM_FEAT_MAX) 870 return (EINVAL); 871 if (feat->id >= nitems(fwcam_feat_ctrl) || 872 fwcam_feat_ctrl[feat->id] == 0) 873 return (EINVAL); 874 875 err = fwcam_read_quadlet(sc, fwcam_feat_ctrl[feat->id], &ctrl); 876 if (err) 877 return (err); 878 879 if (feat->id == FWCAM_FEAT_WHITE_BALANCE) { 880 /* White balance: value=V/R (low 12), value2=U/B (high 12) */ 881 val = (ctrl & 0xff000000) | 882 ((feat->value2 & 0xfff) << 12) | 883 (feat->value & 0xfff); 884 } else { 885 /* Preserve upper bits, set new value in lower 12 */ 886 val = (ctrl & 0xfffff000) | (feat->value & 0xfff); 887 } 888 889 return (fwcam_write_quadlet(sc, fwcam_feat_ctrl[feat->id], val)); 890 } 891 892 static int 893 fwcam_cdev_ioctl(struct cdev *dev, u_long cmd, caddr_t data, 894 int fflag, struct thread *td) 895 { 896 struct fwcam_softc *sc = dev->si_drv1; 897 struct fwcam_mode *mode; 898 struct fwcam_feature *feat; 899 struct fwcam_info *info; 900 int err; 901 902 if (sc->fwdev == NULL) 903 return (ENXIO); 904 905 switch (cmd) { 906 case FWCAM_GMODE: 907 mode = (struct fwcam_mode *)data; 908 mode->format = sc->cur_format; 909 mode->mode = sc->cur_mode; 910 mode->framerate = sc->cur_framerate; 911 mode->frame_size = sc->frame_size ? 912 sc->frame_size : fwcam_frame_size(sc); 913 return (0); 914 915 case FWCAM_SMODE: 916 { 917 int was_streaming = 0; 918 919 mode = (struct fwcam_mode *)data; 920 if (mode->format > 7 || mode->mode > 7 || mode->framerate > 7) 921 return (EINVAL); 922 923 if (!(sc->formats & (1 << (31 - mode->format)))) 924 return (EINVAL); 925 if (!(sc->modes[mode->format] & (1 << (31 - mode->mode)))) 926 return (EINVAL); 927 if (!(sc->rates[mode->format][mode->mode] & 928 (1 << (31 - mode->framerate)))) 929 return (EINVAL); 930 931 FWCAM_LOCK(sc); 932 if (sc->state == FWCAM_STATE_DETACHING) { 933 FWCAM_UNLOCK(sc); 934 return (ENXIO); 935 } 936 if (sc->state == FWCAM_STATE_STREAMING) { 937 was_streaming = 1; 938 FWCAM_UNLOCK(sc); 939 fwcam_iso_stop(sc); 940 FWCAM_LOCK(sc); 941 if (sc->state != FWCAM_STATE_DETACHING) 942 sc->state = FWCAM_STATE_PROBED; 943 } 944 FWCAM_UNLOCK(sc); 945 946 err = fwcam_write_quadlet(sc, IIDC_CUR_V_FORMAT, 947 (uint32_t)mode->format << IIDC_CUR_V_SHIFT); 948 if (err == 0) 949 err = fwcam_write_quadlet(sc, IIDC_CUR_V_MODE, 950 (uint32_t)mode->mode << IIDC_CUR_V_SHIFT); 951 if (err == 0) 952 err = fwcam_write_quadlet(sc, IIDC_CUR_V_FRM_RATE, 953 (uint32_t)mode->framerate << IIDC_CUR_V_SHIFT); 954 955 if (err == 0) { 956 sc->cur_format = mode->format; 957 sc->cur_mode = mode->mode; 958 sc->cur_framerate = mode->framerate; 959 mode->frame_size = fwcam_frame_size(sc); 960 } 961 962 if (was_streaming) 963 fwcam_iso_start(sc); 964 return (err); 965 } 966 967 case FWCAM_GFEAT: 968 feat = (struct fwcam_feature *)data; 969 return (fwcam_get_feature(sc, feat)); 970 971 case FWCAM_SFEAT: 972 feat = (struct fwcam_feature *)data; 973 return (fwcam_set_feature(sc, feat)); 974 975 case FWCAM_GINFO: 976 info = (struct fwcam_info *)data; 977 info->formats = sc->formats; 978 info->basic_func = sc->basic_func; 979 info->features_hi = sc->features_hi; 980 info->features_lo = sc->features_lo; 981 info->cur_format = sc->cur_format; 982 info->cur_mode = sc->cur_mode; 983 info->cur_framerate = sc->cur_framerate; 984 info->state = sc->state; 985 info->frame_size = sc->frame_size ? 986 sc->frame_size : fwcam_frame_size(sc); 987 info->frame_dropped = sc->frame_dropped; 988 info->iso_channel = sc->iso_channel; 989 info->_pad[0] = info->_pad[1] = info->_pad[2] = 0; 990 return (0); 991 992 default: 993 return (ENOTTY); 994 } 995 } 996 997 998 static int 999 fwcam_probe(device_t dev) 1000 { 1001 struct fw_unit *unit; 1002 1003 unit = fw_get_unit(dev); 1004 if (unit == NULL) 1005 return (ENXIO); 1006 1007 if (unit->spec_id != CSRVAL_1394TA) 1008 return (ENXIO); 1009 if (unit->sw_version != CSR_PROTCAM130 && 1010 unit->sw_version != CSR_PROTCAM120 && 1011 unit->sw_version != CSR_PROTCAM104) 1012 return (ENXIO); 1013 1014 device_set_desc(dev, "IIDC Digital Camera over FireWire"); 1015 return (BUS_PROBE_DEFAULT); 1016 } 1017 1018 static int 1019 fwcam_attach(device_t dev) 1020 { 1021 struct fwcam_softc *sc; 1022 struct fw_unit *unit; 1023 uint32_t cmd_base; 1024 1025 unit = fw_get_unit(dev); 1026 if (unit == NULL || unit->fwdev == NULL) 1027 return (ENXIO); 1028 1029 cmd_base = fwcam_find_iidc_cmd_base(unit); 1030 if (cmd_base == 0) { 1031 device_printf(dev, "no IIDC command base in unit directory\n"); 1032 return (ENXIO); 1033 } 1034 1035 sc = device_get_softc(dev); 1036 sc->fd.dev = dev; 1037 sc->fd.fc = fw_get_comm(dev); 1038 mtx_init(&sc->mtx, "fwcam", NULL, MTX_DEF); 1039 1040 sc->fwdev = unit->fwdev; 1041 sc->cmd_hi = 0xffff; 1042 sc->cmd_lo = 0xf0000000 | (cmd_base << 2); 1043 sc->iso_speed = min(unit->fwdev->speed, FWSPD_S400); 1044 sc->state = FWCAM_STATE_IDLE; 1045 sc->dma_ch = -1; 1046 sc->iso_active = 0; 1047 sc->open_count = 0; 1048 knlist_init_mtx(&sc->rsel.si_note, &sc->mtx); 1049 TASK_INIT(&sc->probe_task, 0, fwcam_probe_task, sc); 1050 1051 sc->cdev = make_dev(&fwcam_cdevsw, device_get_unit(dev), 1052 UID_ROOT, GID_OPERATOR, 0660, "fwcam%d", device_get_unit(dev)); 1053 sc->cdev->si_drv1 = sc; 1054 1055 return (0); 1056 } 1057 1058 static int 1059 fwcam_detach(device_t dev) 1060 { 1061 struct fwcam_softc *sc; 1062 1063 sc = device_get_softc(dev); 1064 1065 FWCAM_LOCK(sc); 1066 sc->state = FWCAM_STATE_DETACHING; 1067 wakeup(sc); /* wake any sleeping readers */ 1068 FWCAM_UNLOCK(sc); 1069 1070 taskqueue_drain(taskqueue_thread, &sc->probe_task); 1071 fwcam_iso_stop(sc); 1072 1073 if (sc->cdev != NULL) 1074 destroy_dev(sc->cdev); 1075 1076 seldrain(&sc->rsel); 1077 knlist_destroy(&sc->rsel.si_note); 1078 1079 FWCAM_LOCK(sc); 1080 sc->fwdev = NULL; 1081 FWCAM_UNLOCK(sc); 1082 1083 mtx_destroy(&sc->mtx); 1084 1085 return (0); 1086 } 1087 1088 static device_method_t fwcam_methods[] = { 1089 DEVMETHOD(device_probe, fwcam_probe), 1090 DEVMETHOD(device_attach, fwcam_attach), 1091 DEVMETHOD(device_detach, fwcam_detach), 1092 1093 DEVMETHOD_END 1094 }; 1095 1096 static driver_t fwcam_driver = { 1097 "fwcam", 1098 fwcam_methods, 1099 sizeof(struct fwcam_softc), 1100 }; 1101 1102 DRIVER_MODULE(fwcam, firewire, fwcam_driver, 0, 0); 1103 MODULE_VERSION(fwcam, 1); 1104 MODULE_DEPEND(fwcam, firewire, 1, 1, 1); 1105